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ABSTRACT 



J. I 



Autonomous Underwater Vehicles (AUV) are being 
considered by the U.S. Navy for a variety of missions. 
Requirements for autonomy reinforce the need for a robust 
maneuvering controller that can ensure accurate tracking of 
a planned path. Model reference controllers (MRC) have been 
employed in situations where accurate tracking is desired 
and where plant parameters change with operating conditions. 
Because underwater vehicles are highly non-linear, it is 
conjectured that an MRC will provide improved tracking 
performance for AUVs. This thesis presents the results of a 
simulation study in which the dynamics of a submersible are 
modeled using a modified version of the DTNSRDC 2510 
equations of motion. A linearized version of these 
equations serves as the reference model and provides the 
basis for the design of feedforward and feedback elements of 
the controller. Results show that for dive plane maneuvers, 
accurate tracking of the planned path can be achieved for a 
moderately wide range of vehicle speeds. 
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I. 



INTRODUCTION 



A. GENERAL 

In recent years, the focus on Autonomous Underwater 
Vehicles (AUV's) or, more generally. Unmanned Underwater 
Vehicles (UUV's) has increased. A variety of unclassified 
missions includes Search and Suirvey, Decoy and Outboard 
sensors. Ocean Engineering Work Seirvice, Swimmer Support, 
and Test and Evaluations [Ref. 1] . As the cost of manned 
submarine vehicles increases, there are significant 
advantages to the use of cheaper unmanned vehicles. UUV's 
can be either tethered or untethered. Development in both 
areas is proceeding, but, while tethered vehicles can use 
fiber optic links to human operators on a mother ship, a 
fully autonomous vehicle is required to have a high level of 
intelligent processing on board. Thus the requirements for 
AI and Knowledge-Based Controls are much increased. A 
recent symposium [Ref. 2] has presented a summary of the 
State of the Art in Unmanned Untethered Submersible 
Technology. 

The organization of the intelligent control of an AUV 
can be expressed as a cycle of Sensing, Thinking, and Acting 
(Figure 1.1). At the highest level of the control 
architecture, the mission planning and symbolic reasoning 
lead to requirements for path planning and control. The 
lower level of Acting involves operation and control of all 
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Figure 1.1 Organization of Intelligent Control 
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vehicle inodes of behavior. At the Sensing level, all 
information concerning the environment surrounding the 
vehicle, as well as its own internal state of health, is 
directed to the higher level. Figure 1.1, reproduced here 
from [Ref. 2] illustrates the idea, and Figure 1.2 
illustrates the hierarchical nature of the intelligent 
controls required. 

Part of the sensing and reflexive acting at the lowest 
level involves a high degree of servo-control over all six 
degrees of freedom of the vehicle motion. To effect proper 
control, not only must the autopilot be capable of accurate 
course and depth control, but also, commands for reflexive 
actions for avoidance or attack must be followed accurately. 
These commands can also include hovering while some form of 
underwater work is done. 

B. AIM OF THE STUDY 

This thesis is concerned with the lowest level of 
control — the control of vehicle reflexive maneuvers. It is 
assumed that the planning level control in Figure 1.2 
recognizes the need for evasive action and decides on 
parameters such as speed, course, and depth changes to be 
rapidly implemented. These parameters are then fed to a 
series of stored maneuvers within the framework of a model 
based autopilot system. Figure 1.3 illustrates the concept 
of the "bag” of maneuvers as interfaced to the vehicle 
autopilot. The control concept proposed here is that of a 
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Figure 1.2 Hierarchical Control Structure 
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Figure 1.3 Schematic of Supervisory Type Control Scheme 
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developed for such maneuvers can be in the form of 
algorithms that provide a command generation system to the 
autopilot. 

The purpose of this work is to determine the feasibility 
and the autopilot design methodology for: 

1. the command generation logic, and 

2. a model following autopilot control. 

C. METHOD OF APPROACH 

Since this work deals strongly with underwater vehicle 
dynamics and control, but not with the vehicle hydrodynamics 
per se, it was important to use an existing vehicle model as 
the basis for the work. Such a model (Figure 1.4) was 
provided by [Ref. 4] where the verification of the model by 
experimental data illustrated the reasonableness of its 
coefficients . 

Using the equations of motions of the vehicle, the 
development of command generation logic, the design of the 
model following autopilot, and the AUV maneuvering 
performance, have been accomplished with computer 
simulation. Heavy use of the DSL (Dynamic Simulation 
Language) has been made. [Ref. 5] 

The vehicle selected as the basis for the study is 
approximately 17 feet long and has been simulated over a 
range of speeds from 3 to 30 feet per second where a 
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Figure 1.4 The AUV Geometry Selected 



specific maneuver — a rapid dive to 100 feet — has been the 
focus for the command generation model. 

While much remains to be done, the concept proposed 
appears worthy of future work. 
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II. VEHICLE DYNAMIC MODELING 



A. GENERAL 

This chapter describes the dynamics of a selected AUV. 
The three dimensional motion of an underwater vehicle is 
fully defined using two coordinate reference systems. 

1. Body Fixed Coordinate Reference System — Figure 2.1. 

2. Inertial Reference System — Figure 2.2. 

The vehicle equations of motion are presented and how 
they were modified to suit the needs of an AUV. Also 
included as part of this chapter is a description of the 
derivation of the hydrodynamic coefficients and a brief 
discussion of the propulsion plant and crossflow drag 
modeling. 

B. COORDINATE SYSTEMS 

Three dimensional motions of underwater vehicles are 
normally described using two coordinate reference frames. 

The first is a right-handed orthogonal coordinate system 
fixed in the body. The second, an inertial reference frame, 
is used to define translational and rotational motions in 
global coordinates (Figure 2.1) 

The body fixed coordinate reference frame has its origin 
fixed to the body center and is aligned with the body axis 
of symmetry. Components of the vehicle motion relative to 
this body fixed reference frame are defined as: 
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Figure 2.1 Body Fixed Coordinate Reference Frame 
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Figure 2,2 Inertial Reference Frame 



u,v,w components along the body fixed axes of the 
velocity of the origin relative to the fluid 
(Surge, Sway and Heave velocity respectively) . 

p,g,r components along the body fixed axes of the 
angular velocity of body relative to the 
inertial reference system (Roll, Pitch, and Yaw 
rates) (Figure 2.2). 

The inertial reference frame is also a right handed 
orthogonal coordinate system in which the position and 
orientation of the vehicle's coordinate system is specified. 
The orientation of the body-fixed coordinate system is 
described by Euler angles ip (yaw) , 9 (pitch) , (p (roll) . 

The transformation from body-fixed to inertial is then given 
conveniently by an XYZ rotation sequence ((p , d/ip) • 

Position of the body-fixed coordinate system is then 
expressed in X, Y, and Z coordinates as illustrated in 
Figure 2.1. Orientation of the vehicle's coordinate system 
is expressed in Euler angles ©, ip. 

C. RIGID BODY DYNAMICS AND EQUATIONS OF MOTION 

The equations of motion for a six degree of freedom 
submarine vehicle are now standardized being first fully 
developed by Gertler and Hagen. [Ref. 6]. These equations 
are commonly known today as the DTNSRDC 2510 equations of 
motion. 

Modifications to these standard equations are then 
generally made to reflect the particular hydrodynamic 
characteristics and properties of the underwater vehicle 
being considered. [Ref. 6] Among the most significant 



12 



changes/allowances considered for the AUV in this study 
included an integral formulation of the viscous crossflow 
forces and moments, addition of the effect of an external 
current and perhaps the most significant difference is the 
change made due to the non-conventional shape of the AUV. 

The AUV considered here is peculiar in that its shape is 
more of low aspect ratio wing than that of the conventional 
body of revolution. [Ref. 4] Additional modifications were 
also made by the separation of the coupled input for bow and 
stern planes and also the decoupling of the bow planes so 
that purposely induced roll control could be included. 

The equations of motion for the six degree of freedom 
AUV are listed in Appendix A, in the following form: 

M X = f(x,z,u) (2.1) 



where , 



M = MASS MATRIX 

X = [u,v,w,p,q,r]T 
f = [Fx,Fy,F2,K,M,N]T 



( 2 . 2 ) 

(2.3) 

(2.4) 



and Fjj, Fy, Fj; are hydrodynamic forces and K, M, N are 
hydrodynamic moments, 
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X = 



“ u 


surge 


V 


sway 


w 


= • heave 


p 


roll 


q 


pitch 


r 


yaw 



= Body Coordinate States (2.5) 



z = 



X 

Y 

Z 

<P 

e 



Position -I 



Orientation ■ 



( 2 . 6 ) 

- Inertial Reference System 



u = 



6r ■ 


rudder angle 


•Sbs 


ST&D bow plane angle 




= port bow plane angle 




stern plane angle 


«rm 


delta form 




delta buoyancy 



control 

vector 



(2.7) 



and u is distinguished by context from u — surge velocity of 
the vehicle relative to the surrounding water, or U^q for 
the current. 

In addition to the six equations of motion that define 
the AUV's motion relative to the body fixed coordinate 
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system, six additional equations are required to fully 
specify the vehicle's motion in space. These kinematic 
relations (see Appendix A) specify the position and 
orientation of the body coordinates with respect to an 
inertial reference frame as established by the XYZ 
rotations, and are expressed in terms of linear velocities 
and Euler angular rates. 

D. HYDRODYNAMIC COEFFICIENTS 

Although development of the hydrodynamic coefficients is 
not a thrust of this thesis, a brief description of their 
derivation is warranted. 

The hydrodynamic coefficients provide the source of the 
behavioral characteristics, and thus the responsiveness, of 
a particular underwater vehicle. 

These coefficients are the result of a Taylor series 
expansion, in which only the first order terms are saved, 
based on the motion variables of the hydrodynamic forces and 
moments. The hydrodynamic coefficients are non- 
dimensionalized and can be considered constants within 
limited operating ranges. [Ref. 6] 

There are currently two primary methods utilized for 
obtaining hydrodynamic coefficients. The first is based on 
tow tank experiments using planar motion, and rotating arm 
mechanisms. The second is a geometric analytical approach 
using semi-empirical techniques. [Ref. 4] 
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The coefficients used for this thesis are those that 
were determined using the analytic approach for an SDV 
simulator. [Ref. 4] 

The coefficients thus selected were chosen because of 
convenience and availability rather than any particular 
desirability of the hydrodynamic characteristics implied. 

E. PROPULSION AND CROSSFLOW DRAG MODELING 

1. Propulsion Plant Modeling 

In NCSC's report by Crane, Summey and Smith [Ref. 

4], propulsion plant modeling is discussed. In that report 
they list the effects of propulsion on the motion of a 
sxibmersible. 

propulsion thrust 
propeller slipstream effects 
propulsive torque 
propulsion induced hull effects 
Of these four effects only the first two are considered 
substantial and the last two are considered negligible. 

The propulsive thrust equation was derived by NCSC 
by curve fitting experimental data and the propeller 
slipstream effects are modeled as a function of vehicle 
speed, propeller rpm, and geometry. [Ref. 4] 

2 . Crossflow Drag Modeling 

Since the AUV geometry selected in this study is 
essentially a low aspect ratio wing design and not a body of 
revolution, its body cross-sections are nearly rectangular 
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rather than circular. Because of this an integral strip 
theory formulation of crossflow forces and moments was 
developed and incorporated into the equations of motion as 
given in Appendix A. 

F. BOW PLANE INFLUENCE 

Bow plane action serves to augment stern plane control 
over pitch motions, but adds to the hydrodynamically induced 
drag on the vehicle. When port and starboard bow planes are 
separately controlled, active control over vehicle roll 
motion may be accomplished. Thus the coefficients relating 
to the heave and pitch motions, axial drag, and roll motions 
have been modified here to allow separate active roll 
control . 
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III. LINEARIZATION OF THE VEHICLE EQUATIONS OF MOTION 



A. GENERAL 

The overall objective of this chapter is to fully 
describe the techniques used to linearize the highly non- 
linear equations of motion. A step by step and term by term 
development of the linearized equations are presented and 
all variables are completely specified in their relation to 
the AUV in this study. 

A description of the linearization point and the 
ramifications of linearization about a straight line path is 
also considered in this section. 

B. LINEARIZATION PROCEDURES 

Linearization of the vehicle dynamics is required for 
the design of the vehicle control system. The linearized 
equations also serve as the model reference for the 
controller. The desired form is the state space 

representation of the equations of motion given as, 

M AX = A Ax + B Au (3.1) 

As discussed in Chapter II, the vehicle dynamics are 
represented in the following form: 

M X = f(X/Z,u) (3.2) 
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where M is the mass matrix, x is the time derivative of the 
state vector x and u is the input vector. For the immediate 
purpose at hand, z may be considered to be part of the state 
vector X. Proper separation will be discussed in what 
follows. 

Linearization is accomplished by a Taylor series 
expansion about a nominal path or trajectory given generally 
t>y (Xq (t) ,Uq ( t) ) , with only the first order terms being 
retained. The following form is then obtained: 



. . 8f(x ,u )Ax 

M Xq + M AX = f (X q/IIo) + 



3u 



(3.3) 



where, if ax = (x - Xq) , and AY = (Y “ Uq) > Equation 

(3.3) becomes. 



M AX 



9f(j^,u^)Ax 

8x 8u 



(3.4) 



Defining A = ^ and B = ^ the desired 

state space form is obtained. 

C. APPLICATION TO VEHICLE MODEL 

The state space model is a 12 state model that can be 
separated into two state vectors x and z. The state vector 
X represents the three linear velocities and corresponding 
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three angular rates about an orthogonal coordinate system 
fixed in the body as defined in Equation (2.5). 

The state vector z represents the six kinematic 
relations, three coordinate positions and three Euler angles 
and is defined in Equation (2.6). 

The two sets of six equations that result are of the 
form: 



The control vector u is the input vector and is defined 
by Equation (2.7) . 

By combining both state vectors, the model state vector 
is defined. 



Once the model state vector and control vector are 
defined, the A matrix and B matrix must be determined. The 
A matrix formulation is represented. 



X = M“^f(x,^,u) 



(3.5) 



2 = g(x,z) 



(3.6) 



= [uvwpqrXYZ(j)e 



(3.7) 





A 



(3.8) 



12 X 12 






- 2 - '- 0-0 






-2. -O'-O 



9z 
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and by similar formulation the B matrix is. 



B 

12 X 6 



% 






( 3 . 9 ) 



An element by element formulation of the A and B 
matrices are complex and require careful attention. The 
particular functional form of the derivative expressions 
can, however, be obtained analytically and depending on 
whether Xq and Zq are time dependent or constant, the 
analytical derivatives become time variant or not. For the 
case of linearization about a straight line flight path, 
these derivatives are constant which makes the control 
computations easier than for more complicated nominal flight 
conditions . 

D. LINEARIZED VARIABLES ABOUT STRAIGHT FLIGHT PATHS 

One convenient feature concerning the linearization 
about a straight flight path with forward speed, Uq, is that 
A and B become constant matrices where the coefficients are 
relatively simple functions of the forward speed. Also, 
since the nominal path is associated with neither rotation 
nor cross-track or depth translation, the incremental 
variables Ax and Au are identical to the actual variables x 
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and u except for the longitudinal velocity and position. 
The linearized dynamics in the axial direction become: 




Hb 



(3.10) 



so that as far as the linearized system dynamics are 
concerned Ax(l) = 0 and Ax(t) = Uo(t) . While this feature 
is convenient, it does not provide information on the second 
order effect of control surface action slowing down the 
vehicle. 

A possible approach to alleviating this deficiency in 
the linear model could be to modify the axial direction 

equation of motion so that the drag effects of control 

. 2 
surface action are related to | 62 1 rather than 6 g. This is 

beyond the scope of this thesis. 



22 



IV. AUTOPILOT DESIGN USING OPTIMAL 
CONTROL TECHNIQUES 



A . GENERAL 

This chapter contains a review of optimal control 
techniques as developed and used in this study for the 
control of autonomous underwater vehicles. Such an 

autopilot has been classically treated as a series of 
interconnected feedback loops for independent control of 
depth and control of course and heading, while roll control 
of the vehicle has been left passive. Control of the sixth 
degree of freedom, longitudinal velocity, has not been 
considered important and a constant thrust or propeller 
speed has been assumed. 

While control of all six degrees of freedom may be 
important in the end for future AUV operations, and 
particularly in the transition from cruise to hover modes, 
this is not the primary focus here. Instead, this chapter 
deals with the state of the art in systems concepts for 
underwater vehicle course and depth control, together with a 
review of the modern multivariable system controls methods 
used in modern autopilot design. 

B. CLASSICAL CONTROL OF COURSE AND DEPTH 

Simple autopilots have long been of interest in 
relieving the human operator of onerous tasks and preventing 
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fatigue. Classical design techniques have considered depth 
and course control as separate, non-interacting control 
systems. The depth controller directs commands to the stern 
planes based on an error between pitch angle command and 
vehicle pitch angle where the pitch command is proportional 
to depth error. Course heading controllers provide rudder 
angle commands proportional to heading angle error. Walker 
[Ref. 7] recently proposed the addition of a cross track 
position feedback loop using yaw angle damping to control 
the cross track distance for automatic track control. 

Most vehicle controllers in practice rely on classical 
concepts with protection limits on command signals so that 
control surface commands can be limited in magnitude and 
rate. Adaptive steering controllers have been proposed as 
an extension for course maintenance in heavy seas when 
optimized gain settings are based on calm weather ship 
characteristics [Ref. 8]. The main limitation of autopilot 
designs based on classical concepts are, 

1. Ship characteristics vary strongly with speed so that 
gain settings for all of the major loops have to be 
adjusted to maintain optimum performance under wide 
operating conditions. 

2. Gains set based on maximum actuation limits and 
usually designed to regulate vehicle depth and course 
about nominally fixed reference settings. 

3. Control of depth and course changes (i.e., rapid 
maneuvering) is not easy and usually not automated. 

The control of rapid maneuvering is more suited to the 
more recent multivariable control system structures such as 
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those involving Model Following Controllers, and Model Based 
Compensators as proposed by Milliken [Ref. 9]. 

C. REVIEW OF OPTIMAL CONTROL CONCEPTS RELATING TO 

CONTROLLER DESIGN 

1. LOR Summary and Discussion 

Much has been written about the application of 
Optimal Control Concepts to the design of feedback systems 
for both output regulation and input tracking. Kwaakernaak 
and Sivan [Ref. 10] present a discussion of design methods 
based on state of the art to 1971. Kaufman and Berry [Ref. 
11] have provided examples of autopilot design methods based 
on linear optimal regulator (LQR) methods and model 
following techniques. Milliken [Ref. 9] has showed 

recently, the use of Model Based Compensators for providing 
multi-degrees of freedom control for a submarine depth and 
course control using linear control techniques — similar to 
those used in this work. Most recently, non-linear control 
methods have been proposed by Slotine [Ref. 12], and Yoeger 
and Slotine [Ref. 13] to provide robust trajectory control 
for underwater vehicles. Using linear control procedures, 
the vehicle, or object to be controlled is described by a 
linear state variable dynamic model for response computation 
by equations of the form, 

Xp(t) = Ap Xp(t) + Bp Up(t); Xp(0) given (4.1) 
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in which the matrices Ap and Bp represent constant 
coefficient terms, and Xp(t) and Up(t) respectively, 
represent the vector of motions (positions and velocities) 
and the control actions (control surface deflections) . 

The design of a linear optimal regulator (LQR) 
control is based on the notion that if some non-zero initial 
condition, x(0) , is established, then Up(t) can be designed 
so that the non-zero state values can be reduced to the 
equilibrium values x(t) = 0, x(t) = 0 with a control 
operation given by, 

Mp(t) = - K Xp(t) (4.2) 

where K is found from the minimization of the quadratic 
performance index, 

00 

J = / (x*^ S X + E M) dt (4.3) 

0 

Here, 2 is a non-negative definite square symmetric 
weighting matrix for response magnitudes and R is a positive 
definite square symmetric weighting matrix for control 
effort. 2 is size nxn, and R has rank equal to the number 
of control inputs modeled (r) . 

The solution for K becomes a matrix of size rxn 
found as the solution to. 
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where 



P A + PJ^E + Q - P(B“^K“1#) P = 0 



(4.5) 



The eigenvalues of the closed loop regulator are 
determined from the combined state and co-state system 
equations. They are given by the eigenvalues of the 
composite matrix ^ where, 



SS = 



A 

-Q 



B R“1 b"^ 



-aT 



(4.6) 



It can be show [Ref. 10] that P is also given by, 



P = [W2] 



(4.7) 



where W]^, W 2 "the nxn partitions of the matrix, W, 



W = 



Ml 

H2 



(4.8) 



formed from columns of stable eigenvectors of SS. It has 
also been found that the use of real part and imaginary 



parts of a complex conjugate eigenvector as adjacent columns 
of W where a complex pole pair exist, eliminates the need 
for complex matrix inversion [Ref. 10]. 

The design by minimization of J in Equation (4.3) 
yields the closed loop control system equations, 

Xp(t) = (A - B K)Xp(t); Up = -K Xp,- Xp(0) given (4.9) 

where the steady state response is zero for both Xp and Up. 

The state vector may, in many cases, be considered 
as a deviation vector from a desired constant level, and it 
is quite appropriate for the steady state values of Xp and 
up to go to zero. However, in the reality of some cases, 
the maintenance of a constant level in some elements of the 
state vector requires a non-zero steady state control signal 
level and in these cases Up(a>) = o. If a steady state level 
must be maintained for any element of the state vector, the 
steady state equations are first solved as, 

0 = Ap Xp(c°) + Bp Up(oo) (4.10) 

Equation (4.10), subtracted from Equation (4.1) 
reduces these cases to the equivalent of a regulator control 
problem by shift of variable, 

^p(^) “ ^p(5?p(t) — Xp(oo)) Sp(Hp(t) “ Hp(“)) (4.11) 
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where the new variables 



Xp(t) - Xp(oo) 



(4.12) 



and 



Up(t) - Up(oo) 

are related in a control law 

%(t) - Mp(“) = -K(Xp(t) - Xp(o°)) 



(4.13) 



(4.14) 



or 



Up(t) = Up(oo) - K(Xp(t) - Xp(oo)) (4.15) 

The above discussion has been limited to 
deterministic signals and to the assumption ' that all 
physical state variables are either measurable or determined 
in a full state observer [Ref. 10]. 

Where the output of the controlled process is to be 
regulated, the above techniques may be used to design the 
elements of the feedback gain matrix thus avoiding the 
complex task of designing separate control loops from each 
variable in the process. The method is powerful, but 
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requires skill in the selection of appropriate £ and R 
factors. 

2 . Tracking Control Systems — (LMFC + MRAC) 

Where the control system is required to drive a 
process so that the output tracks an input variable within 
acceptable error bounds, the problem is further compounded. 
Even more difficult is to achieve the tracking of several 
simultaneous inputs by the various outputs of the driven 
process. During the late 1960 's and early 1970 's, much 
attention was placed on linear model following controls 
(LMFC) and model reference adaptive controls (MRAC) to 
provide the acceptable tracking behavior of multivariable 
systems. Kaufman and Berry [Ref. 11] described the 
application to flight control, and Landau [Refs. 14,15] gave 
a survey of design techniques and system structures in which 
it became clear that a model of the system to be controlled 
was needed to represent the desired time behavior of the 
system state variables. The system control variables then 
became a function of the input variables to the model, and 
the model state variables, in addition to the feedback of 
system state variables. Thus better information than could 
be derived by feedback was used to drive outputs to track 
inputs . 

The use of MRAC techniques allows for not only model 
following, but also the provision of adapting gains, or 
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model parameters in achieving precise control when system 
operating conditions change. 



One of the difficulties pointed out by Landau [Ref. 
14 ] , is that controller parameters need to change when the 
plant operating conditions changed. Thus using a reference 
model not only provides the robustness achieved by 
predictive and corrective control but also provides the 
opportunity to update model and control parameters 
automatically . 

Restricting the discussion to Linear Model Following 
Controls (LMFC) , the control issues are analyzed as follows: 
the plant model is given by, 





— P ~ — P — P — P 


(4.16) 




^P “ — P — P 


(4.17) 


and a 


suitable model of the plant, but with 


■ desirable 


dynamic 


response characteristics (response time. 


stability. 


etc.) is given by, 






^m = Am Xm + Bji, u^(t) 


(4.18) 




m 

um = 0 


(4.19) 




II 

lo 

IX 


(4.20) 
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then the control signals, which minimize the weighted 

integral of errors between model and plant are given by, 

Up = Ki um + K 2 Xm + K 3 Xp (4.21) 

where the errors are defined as, 

e = Ym - Yp (4.22) 

and the performance index minimized is, 

00 

J = / (e'^ £ + Un*^ E Up) dt (4.23) 

0 t' t' 

and fi, and R are weighting matrices as discussed earlier. 

The computation of the gain matrices, K^, K 2 / and K 3 
are fortunately made easier by considering the combined 
system, model plus plant as a coupled linear system. ' Also, 
to overcome problems that arise when the signals to be 
tracked, Yui(t) are derived from inputs, Ujjj(t) , that are not 
impulses, it is convenient to consider that the additional 
model equations. 

Mm = 0 (4.24) 

be incorporated together with u^ as a composite state 
vector. 
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(4.25) 



A LiiD /iiin 'J^in J 



to get the system equations. 



IX* 
1 




-P 


0 


0 


^m 


= 


0 


^m 


Bm 






0 


0 


0 



Now, application of the LQR 
system given above yields. 




technique to the composite 



Up = -[R-1 BpT)[P][2£p Xj, U„)T 



(4.27) 



where. P now is of dimension (np + nm + rm) , and [R~^ b'^ P] 
is partitioned in three parts. 



[R"^ bT P] = [K 3 K 2 Ki] (4.28) 

By varying the weighting factors within the matrix, 
Q, selected errors may be penalized more heavily than others 
in the optimal control trade-off. Also, selection of 
parameters within R may be used to provide a trade-off 
between a sluggish or sensitive control design. Details of 
numerical values used in the design of the autopilot 
controls are given later in Chapter VI. 
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3 . Near Time Optimal Maneuvering Models 

The use of the system structure implied by Equation 
(4.26) and the resulting control law. Equation (4.21), is 
particularly useful when considering near time optimal 
positioning of inertial objects. It is well known that time 
optimal position control of a massive object requires a 
bang-bang application of force or torque. These concepts 
are recently being considered in robot tracking control 
[Ref. 16], and the sliding control described in [Ref. 17]. 
So also, in the field of LMFC for underwater vehicle 
maneuvering control , it is expected that rapid maneuvering 
will require some form of bang-bang operation of control 
surfaces. Bang-bang operation, in principle, is simple, 
consisting of a sequence of stepwise control actions, yet 
knowledge of switching times for anything other than very 
low order systems make the principle difficult to implement. 
The outcome of the above discussion then leads to the 
development of vehicle maneuvering models based on use of a 
series of constant setting for control surfaces that make Up 
the model input vector Uj^. At times during the response of 
the model where switching should occur, the control surfaces 
change setting rapidly as if by imposition of an impulse 
command. Therefore, if it is considered that surface 
settings change levels at discrete but arbitrary times, the 
unforced nature of the model reference states, in Equation 
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(4.26) are preserved and the application of the LMFC system 
is valid. 

For every reflexive maneuver envisioned during the 
operation of an AUV life, it is foreseen that maneuvering 
logic can be developed on an algorithmic basis to determine 
switching times, using logic to be developed and the Ajj^, Bjjj, 
Ki, K 2 and K 3 matrices as shown in Figure 4.1. These data 
can be stored inside on-board processors so that on coiranand 
from the high level controller or expert system, new 
computations for Up can be implemented immediately. 

The development of a maneuvering logic as a command 
generation system for a dive maneuver will be discussed in 
more detail . 
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Figure 4.1 Block Diagram for a Model Following Autopilot 



V. SIMULATION TECHNIQUES 



A. GENERAL 

This chapter contains a discussion of the computational 
program structures used in this study. All simulations were 
performed using the Dynamic Simulation Language (DSL) [Ref. 
5] code for the simulation of linear and non-linear system 
response as a function of time. Internal numerical 
integration routines make this aspect of the solution 
transparent to the user. The user provides only the details 
of the particular equations employed. In this work, DSL was 
used for the simulation of both uncontrolled and autopilot 
controlled vehicle responses. However, as part of the 
design procedure for the autopilot, the complete set of 
feedforward and feedback gains were established using ETAT — 
a specially developed program for the computation of linear 
optimal control gains. The pertinent linkages between DSL 
and ETAT were developed and implemented during this study. 
More detailed descriptions follow. 

B. COMPUTATION OF FEEDFORWARD AND FEEDBACK GAINS 

While the theory behind the need for feedforward gains 
for optimal model following autopilots has been given in 
Chapter IV, this section discusses the program organization 
used in their computation. 
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The outline organization of program ETAT is shown in 
Figure 5.1. ETAT reads and writes values of and as 

computed within the framework of the DSL simulation and also 
reads user input values for the tracking error weighting 
matrix, Q, and the control input weighting matrix, R. 
Particular values used for Q,, and- R, are given later in 
Chapter VI. 

Subroutine MTXEXP computes the matrix exponential 
associated with and the discrete time input matrix 

associated with ^ and M, but this section has not been 
used here. 

Subroutine ROOTS is used for the computation of both 
eigenvalues and eigenvectors of a square matrix (^) , and 
calls the IMSL double precision library routine EIGRF and 
its associated subroutines. 

OPTIMA is the subroutine used for assembly of the 
composite state and co-state matrix, OPTIMA also calls 

EIGRF and computes the closed loop system eigenvalues and 
vectors. These, as given earlier in Chapter IV, are used to 
form the solution of the matrix Ricatti equation and the 
overall matrix of gains, i.e.. Equation (4.4). Partitions 
of the overall gain matrix give the individual matrices, K^, 
K 2 and K 3 in Equation (4.28). 

A listing of the major subroutines used in program ETAT 
are provided in the appendix for the interested reader, 
although use of ETAT without. proper linkages to DSL and the 
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Figure 5.1 ETAT Flow Diagram 
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appropriate IMSL double precision library would not be 
proper . 

C. REFERENCE MODEL DEVELOPMENT 

As discussed earlier, the reference model is a full 12 
state representation of the AUV. The reference model can be 
represented by: 

X = AA X u ju u lu =0 (5.1) 

A computational problem in subroutine OPTIMA can arise 
because of the multiple zero eigenvalues associated with 
several of the modes in the above equations. This problem 
has been overcome here by inserting very small values, - 
~(^)i» the key diagonal elements of the ^ matrix so that 
distinct eigenvalues result. Since the ( A) values are 
extremely small, their effect on the system poles is 
negligible and the problem of multiple repeated poles is 
eliminated. 

It is conceivable to have a series of reference models, 
one for each of several reflexive maneuvers. Each maneuver 
will have its associated logic that will generate the 
control input to the model and thus provide the model 
reference states. 

For this thesis, only one such maneuver, a dive 
maneuver, was investigated. Logic for the dive maneuver is 
based on an application of bang-bang optimal control theory. 
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thereby yielding time optimal response. The methodology 
here is to deflect stern planes up and the bow planes down 
to initiate a pitch rate (p) until the vehicle achieves some 
predetermined pitch angle (0). For what is considered 
reflexive, or emergency obstacle avoidance, a large angle is 
desired. Assuming that the submersible is directionally 
stable, some small stern plane angle must then be maintained 
to keep a constant pitch angle, dependent on speed, until 
such time when the control action should provide an opposite 
effect to come out of the dive and steady at a new depth. 
An example of this control action is shown in Figure 5.2. 

Given limits on control surface deflection and maximum 
pitch angle during the dive, this type of control action 
should provide an optimal response for a change in depth. 

With this control logic preprogrammed into an AUV, 
whenever the supervisory control system calls for a dive 
maneuver, the logic can provide the control input for the 
reference model and thus an optimal path can be created 
quickly; one that the controller can track and vehicle can 
follow. 

The logic for the dive maneuver is crude, however, this 
is a trade-off for ease in programming the algorithm used 
for the dive maneuver. When programming one of these 
reflexive maneuvers one must be cautious not to program a 
maneuver that is beyond the capability of the vehicle. 
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Time Optimal Control Action for a 
Dive Maneuver Command Generation 
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A conceptual objective is to have many maneuver 
algorithms preprogrammed into a vehicle into a "bag" of 
maneuvers. This bag of maneuvers would be at the disposal 
of the supervisory control. This supervisory control would 
be the manager of the bag of maneuvers, as earlier indicated 
in Figure 1.3, and would receive its instructions from the 
on-board expert system or, in the future, artificial 
intelligence. 

For the many types of standard and emergency situations 
required, collision or obstacle avoidance, a proper maneuver 
can be chosen and executed quickly and efficiently without 
excessive computational burdens that would otherwise lead to 
a tardy response. 

D. SYSTEM SIMULATION METHOD 

1. Dynamic Simulation Language (DSL) 

DSL is a Fortran based simulation language for 
digital simulation of continuous systems. DSL uses a 
building -block approach to programming. Programs can be 
very simple or they can become extremely complex when all 
the functions of DSL are utilized. The user can enter 
Fortran statements in any order and DSL can sort and solve 
these equations effectively. The user can also include 
fortran subroutines and use the expansive I/O facility of 
DSL. One other key feature is the integration routine 
capability. The user has the choice of nine integration 
methods; four fixed-step, two variable-step and three 
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variable-step, variable order methods. DSL was chosen 
primarily because it easily can solve differential equations 
and it contains many internal functions that normally would 
have to be programmed by the user. 

DSL has four phases of program execution; 

TRANSLATION 

COMPILATION 

SIMULATION 

GRAPHICS 

DSL translates all the DSL code into Fortran 
statements. Once the code is translated, it is then linked 
to the VS compiler and the code is compiled and stored as an 
executable file. Upon completion of the compilation phase, 
the simulation phase begins, and the system clock starts, 
and simulation continues until the system reaches its user 
specified finish time. The last phase of problem execution 
includes the graphic capability of DSL. Saved output data 
can be plotted or graphed using the graphics post-processor 
and the specific hardware supported. 

2 . Problem Simulation 

As mentioned earlier, DSL uses a building-block 
approach to programming. The major blocks and general flow 
of program simulation are shown in Figures 5.3 and 5.4. To 
fully understand the simulation, and the controller design, 
and control action, a detailed breakdown and discussion is 
required. 
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Figure 5.4 DSL Simulation Flow Diagram Continued 
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The first section of the simulation is the CONSTANT 
block. In this block all of the hydrodynamic coefficients 
and vehicle constants are read into the program. 

The second section is the INITIAL block. In this 
section, all of the calculations not part of the integration 
routines and those needed in establishing parameters and 
initial conditions are performed. This is also where all 
variables are initialized. The following calculations occur 
in this section; 

1. All matrices and arrays are initialized to zero. 

2 . The length and weight fractions for a four term gauss 
quadrature are initialized. 

3. The breadth and height terms are read in. These terms 
will be used in the gauss quadrature integration for 
the crossflow drag terms. 

4. The thrust is then calculated for the propulsion 
model . 

5. The non-zero elements of mass matrix M are calculated. 

6. The square mass matrix M has rank of six is then 
inverted using the IMSL routine LINV2F. 

7. The non-zero elements of the A matrix are calculated. 
These elements are the coefficients of the first order 
terms in the Taylor series expansion about a specific 
operating point. 

8. The non-zero elements of the B matrix are then 
calculated. 

9. The next step is to multiply the inverse of the mass 
matrix to obtain the coefficients of the state 
equation, (5.1). 

10. The last task for the initial section is to read in 
the computed feedforward and feedback gains from the 
program ETAT that are to be used in the autopilot 
control law. 
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The third block of the main program is called the 
DERIVATIVE section. Here, all the first order equations 
that must be integrated and solved are assembled. The 
DERIVATIVE section of this simulation is comprised of three 
major subsections, one for each major section in the 
simulation. 

1. Linear reference model providing command generation. 

2. Control law linking model and vehicle response to 
control surface actions. 

3. Nonlinear model for simulating vehicle response. 

The control vector Uj^^ is the input to the linear 
model, generated from the maneuver logic contained within 
the DYNAMIC section. This section will be discussed later. 

Once the control input is established, the 
derivative expressions of the linear reference model are 
formulated in terms of the matrices AA j^ and BEj^. 

After the linear model derivatives are established, 
the model states and model inputs Ujjj are passed to the 
Control Law. The Control Law (Equation (4.21)) represents, 
in software, the gains that would be incorporated in the 
vehicle. 

The input vector Up represents the inputs to the 
actual vehicle, in this case, defined by Equations (3.2). 

The derivatives of the vehicle state variables are 
formed as the last part of the DERIVATIVE section in 
preparation for numerical integration using the fifth order 
variable step Runge-Kutta technique. 
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The model states and inputs, as well as the vehicle 
states and inputs, are saved for graphical representation 
and data output. 

The fourth major block of the simulation is the 
DYNAMIC section. In the DYNAMIC section, the maneuver logic 
is programmed. This section is reserved for those 
computations that depend on time and are independent of the 
system responses. However, response dependent functions may 
also be included here as is the case with the establishment 
of the reference control commands generated by the maneuver 
logic. 

The fifth section of the program is the CONTROL 
section. Before the command or input is sent to the 
derivative section, the system time clock is checked, and if 
"finish time" (f intime) in the CONTROL section is reached 
the program stops. If not, the system increments itself one 
time step and continues with the simulation. 

Upon completion of the simulation a time history of 
all desired parameters and variables are saved in a data 
file. Plots and graphical output may then be generated. 

3 . Procedure Used 

To perform a simulated run with a particular 
autopilot design and vehicle speed, an initial run with DSL 
was required to establish values for the and BB 
matrices. These values were written on as output files 
(file FtlOFOOl for M and file Ft09F001 for BB) . By 
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separate run, program ETAT was used to read ^ and and 
its own input file FtOSFOOl for Q and R and to provide 
values for control gains K 2 and K 3 . The gain matrices, 
written on file Ft02F001 were then read by a final run using 
DSL for the controlled vehicle response simulation and 
results were provided on data file OUTP. 
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VI. RESULTS 



A. GENERAL 

This chapter describes the efforts and results of the 
design of a model following autopilot for an AUV. The 
controller designed is only a partial solution to the 
complete control over the six degrees of freedom of an AUV. 
However, the methodology developed in this study could be 
applied to design a full 30 state controller, 12 plant 
states, 12 model states, and 6 control states. The 
controller designed in this study is a 19 state controller 
using 12 plant states, 4 model states relating to the pitch 
plane, Wjjj, qj^j, Zjjj, 6jjj and the three control inputs for this 
plane, port and starboard bow plane angle and stern plane 
angle . 

In Section D, the base-line controller is tested and the 
results show excellent depth control with excellent time 
history tracking. However, the control over pitch angle 
appeared loose and in Sections E and F attempts were made to 
gain tighter pitch control. 

A test of controller robustness is .its ability to 
operate over a range of vehicle speeds and changing 
parameters. In Section G the controller was tested at 

speeds of 3, 12 and 30 ft/sec, approximately 1.8, 7 and 17 
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knots, respectively, while baseline runs were at 6 ft/sec 
(3.5 knots) . 

Included in this chapter is a discussion of the gains 
used, how they were derived and the effects on the gains by 
varying the control weight matrix R and the control error 
weight matrix 2. 

B. RESULTS OF UNCONTROLLED MANEUVERING 

The first simulation runs that were made early in this 
study were to test the non-linear model. One maneuver that 
was first tested was a turning maneuver. Because of this 
AUV's particular geometry (low aspect wing vice body of 
revolution) , some unique behavioral characteristics are 
displayed as shown in Figures 6.1 and 6.2, not common to 
other forms of underwater vehicles. Of the most significant 
is when a rudder command is given the vehicle rolls out of 
the turn. While this is not uncommon for vehicles without a 
sail area, it is uncommon for a vehicle with a cruciform 
type stern to dive on a turn while the vehicle rolls out. 
Although this vehicle's dynamics are not representative of 
those common to submersibles, the behavior has been verified 
experimentally. The purpose of selection was based purely 
on the availability and thoroughness to which the vehicle 
dynamics were modeled, and that program validation was 
easily accomplished from the data available. 
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Figure 6.1 Uncontrolled Turn Maneuver for Non-Linear Model Horizontal Plane 
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Figure 6.2 Uncontrolled Turn Maneuver for Non-Linear Model Dive Plane 



C. DIVE PLANE MANEUVER AND PREDICTOR CONTROL 

Once the non-linear model was validated the controller 
design process then began. The first simulation of this 
process consisted of only predictor control, no feedback was 
incorporated. The inputs generated by the dive maneuver 
logic for stern and bow plane input to the linear model were 
also fed into the non-linear vehicle dynamics. 

This run, Figures 6.3 to 6.7, provided insight on the 
accuracy of the linearized version of the equations of 
motion. Figure 6.7 shows excellent pitch correlation 
between the model and vehicle. 

Figures 6.4 and 6.5 both show that the vehicle never 
reaches its ordered depth of 100 ft. because the vehicle 
equations were linearized about a straight line trajectory 
at a constant speed, the linear model does not generate any 
speed loss and subsequently the AUV lags behind the linear 
model, a result that was clearly expected. The 
responsiveness of the vehicle is interesting considering the 
slow speed of 3.5 knots. 

Examination of the maneuver shows that a limit of about 
0.25 radians and 0.18 radians, respectively, was set by the 
command generation logic while the maximum pitch angle of 40 
degrees was reached and maintained after 16 seconds. Also, 
while the vehicle pitch angle is returned to a small value, 
when the control surfaces are returned to zero, a small 
residual pitch angle is left. This is a small point that 
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Figure 6.3 Dive Plane Maneuver and Predictor Control Stern Plane Angle — 6 Ft/Sec 
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Figure 6.4 Dive Plane Maneuver and Predictor Control Dive Profile — 6 Ft/Sec 



o 




58 



Figure 6.5 Dive Plane Maneuver and Predictor Control Depth Vs Time — -6 Ft/Sec 
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Figure 6.6 Dive Plane Maneuver and Predictor Control Bow Plane Angle — 6 Ft/Sec 
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Figure 6.7 Dive Plane Maneuver and Predictor Control Pitch Angle — 6 Ft/Sec 



could be corrected by a refinement of the command generation 
logic. 

What is of interest is the magnitude of the final depth 
error generated by the difference between linear and non- 
linear models. While the command generation logic drives 
the linear reference model to the targeted depth of 100 ft. , 
the speed reduction in the AUV only provides a dive to 87 
feet — clearly indicating the need for corrective control 
action. 

D. EFFECT OF AUTOPILOT CONTROL — BASELINE CASE 

Figures 6.8 to 6.12 clearly show the difference the 
controller makes in attaining the ordered depth. This was 
the first simulation run using the full 19 state controller 
for control in the heave/pitch plane. Although excellent 
depth control was achieved, the pitch control was considered 
a little loose resulting mainly from the mismatch between 
model and AUV speed. Figure 6.12 shows the overshoot of the 
vehicle pitch during the maneuver. The overshoot of the 
pitch also is the primary reason for the vehicle attaining 
ordered depth much more rapidly than the model as shown in 
Figure 6.9. 

Other observations include, the majority of the control 
action comes from the stern plane which worked much harder 
than the bow planes. Figures 6.8 and 6.11 show the 
differences in control actions between the model and vehicle 
for stern and bow planes, respectively. 
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Figure 6.8 Autopilot Control — Baseline Stern Plane Angle — 6 Ft/Sec 
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Figure 6.9 Autopilot Control — Baseline Dive Profile — 6 Ft/Sec 
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Figure 6.10 Autopilot Control — Baseline Depth Vs Time — 6 Ft/Sec 
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Figure 6.11 Autopilot Control — Baseline Bow Plane Angle — 6 Ft/Sec 



QW 


1 

1 

1 

1 










P.ffi 






Oi 




p: 

1 

1 




o 

o 

CM 



O 

CD 



O 

CO 



O 



O 

'CM 



o 

W 
o C/D 
■ o ' 

S 

CD 



O 

CO 



ST- S2- SC- ST^- SS- 

(saaaoaa) aaoNv Hand 



66 



Figure 6.12 Autopilot Control — Baseline Pitch Angle — 6 Ft/Sec 



Because of the disparity in control efforts an attempt 
to equalize the relative amount of control actions and more 
closely following the model was made. As discussed in 
Chapter IV, the control weight matrix R (Table 1) was 
initially set up to penalize the rudder, rpm and buoyancy 
inputs, so that the primary control actions would be from 
the bow and stern planes as it would be for a dive maneuver. 
In this first run the weights were equal and the resulting 
control gains (Table 3) for the stern plane were much higher 
than for the bow planes. An attempt was made at sharing the 
control actions where weights of the R matrix were adjusted 
to penalize the stern plane and put more control effort in 
the bow planes. This resulted in a significant loss in the 
stern plane gain much less that one and only a very slight 
rise in the bow plane gain. Although the resulting 
simulation showed more bow plane action it did not follow 
the model well and the stern plane became more active by the 
feedback action. This increased activity in the bow and 
stern planes resulted in very significant speed loss and 
excessive plane use was considered unacceptable. 

Upon further study of the vehicle and its dynamics, the 
reason for the inconsistency in control actions is that the 
model maneuver treats bow and stern planes almost equally in 
their effect but in fact the force and moment generated by 
the stern plane is an order of magnitude more significant 
than that of the bow planes. Therefore, in future maneuver 
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TABLE 1 



TABLE OF 2 WEIGHTS— BASELINE 
Pitch Rate Error 



Q(5,5) 


0.6 


Q((5,14) 


-0.6 


Q(14,5) 


-0.6 


Q(14,14) 


0.6 


Pitch Anale Errors 


2.5 


Q(ll,16) 


-2.5 


Q(16,ll) 


-2.5 


Q(16,16) 


2.5 


Heave Rate Error 
Q(3,3) 


1.0 


Q(3,13) 


-1.0 


Q(13,3) 


-1.0 


Q(13,13) 


1.0 


Heave Positional Error 
Q(9/9) 


60.0 


Q(9/15) 


o 

• 

0 

VO 

1 


Q(15,9) 


o 

• 

0 

VO 

1 


Q(15,15) 


60.0 
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TABLE 2 



TABLE OF R WEIGHTS — BASELINE 



Rudder 


R(l,l) 


1.0 


X 


104 


Starboard Bow Plane 


R(2,2) 


1.0 


X 


10^ 


Port Bow Plane 


R(3,3) 


1.0 


X 


10^ 


Stern Plane 


R(4,4) 


1.0 


X 


io3 


RPM 


R(5,5) 


1.0 


X 


10® 


Buoyancy 


R(6,6) 


O 

• 

f— 1 


X 


10® 
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. TABLE 3 



CONTROL GAINS 



O.OOOOOOOOOOE+00 
0.<i954081951E-08 
O.OOOOOOOOOOE+00 
O.‘464^40128‘i0E-09 
0.25956690<i2E-05 
-0.<i438610978E-0<i 
-0.5901765^58E-07 
O.OOOOOOOOOOE+00 
0.7605860<i58E-0^ 
O.OOOOOOOOOOE+00 
0.7216705136E-05 
0.<i45^871633E-01 
-0.6952859507E+00 
-0.99872^3305E-02 
O.OOOOOOOOOOE-i-OO 
0.7628508878E-04 
O.OOOOOOOOOOE-i-OO 
0.7236512897E-05 
0.4459061566E-01 
-0.-6970854515E-I-00 
-0.9759326349E-02 
O.OOOOOOOOOOE-i-OO 
-0 .6689594082E-03 
O.OOOOOOOOOOE-i-OO 
-0.5682397825E-04 
-0.6106134440E-01 
0.5192655967E-i-01 
-0.1004805424E-i-01 
O.OOOOOOOOOOE-i-OO 
-0.4904055922E-10 
O.OOOOOOOOOOE-i-OO 
-0.4355770203E-11 
-0.1393062766E-07 
0.4070361389E-06 
-0.4242388239E-07 
O.OOOOOOOOOOE-i-OO 
0.8911601991E-10 
O.OOOOOOOOOOE-i-OO 
0.1128610385E-10 
0.1929779261E-06 
-0.1207295075E-05 
-0.4768694409E-06 



0.7258932550E-10 

0.3019079092E-04 

O.OOOOOOOOOOE-i-OO 

0.4422721241E-04 

-0.3025079273E-04 

-0.1165944258E-05 

0.1114619576E-05 
0.4668891458E-i-00 
O.OOOOOOOOOOE-i-OO 
0.6929793740E-i-00 
-0 .4676730097E-i-00 
-0.1746945879E-01 

0.1117984364E-05 
0 .4682061530E-i-00 
O.OOOOOOOOOOE-i-OO 
0 .6947686167E-i-00 
-0.4689957818E-i-00 
-0.1752998162E-01 

-0.9857068418E-05 
-0.3859172641E-^01 
O.OOOOOOOOOOE-i-OO 
-0.5158026393E-i-01 
0.3880347969E-i-01 
0 .1856029149E-i-00 

-0.7210798044E-12 

-0.2899780644E-06 

O.OOOOOOOOOOE-i-OO 

-0.4049075188E-06 

0.2911221362E-06 

0.1269293021E-07 

0.1283218668E-11 
0 .6522729441E-06 
O.OOOOOOOOOOE-i-OO 
0.1210698470E-05 
-0.6471117447E-06 
-0.6867469453E-08 



-0.2575687868E-05 
0.2760571526E-08 
-0.2752279831E-05 
O.OOOOOOOOOOE-i-OO 
0 .2764935447E-05 
-0.1165974829E-05 

-0.4422518793E-01 
0 .4168961983E-04 
-0.4510255945E-01 
O.OOOOOOOOOOE-i-OO 
0.4530064631E-01 
-0.1747022491E-01 

-0.4426664311E-01 

0.4182848952E-04 

-0.4518982016E-01 

O.OOOOOOOOOOE-i-OO 

0.4538842427E-01 

-0.1753074334E-01 

0.6050596839E-01 
-0.4217829036E-03 
0 .2348149885E-i-00 
O.OOOOOOOOOOE-i-OO 
-0.2363129821E-i-00 
0.1855811163E-i-00 

0.1382443897E-07 
-0 .2934508269E-10 
0.2184067568E-07 
O.OOOOOOOOOOE-i-OO 
-0.2195747233E-07 
0.1269214802E-07 

-0.1916238257E-06 

0.2538939491E-10 

-0.1217387526E-06 

O.OOOOOOOOOOE-i-OO 

0.1220747899E-06 

-0.6880516105E-08 



RUDDER 



PORT 

BOW PLANE 



STBD 

BOW PLANE 



STERN 

PLANE 



RPM 



BUOYANCY 
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model generation it should be noted that bow plane forces 
and moments are more subtle and should be used for fine 
depth control rather than for major depth excursions. 
Considering the speed mismatch, the overall control was 
considered acceptable. 

E. EFFECT OF TIGHTER PITCH CONTROL WEIGHTING 

Due to the unique dynamics of the vehicle it was decided 
to leave the control weights the same in the R matrix, as it 
was in the first run, with the understanding that the model 
maneuver algorithm perhaps wasn't as well suited for this 
vehicle as it could have been. 

With the R matrix fixed, with equal weights on the bow 
and stern planes, it was decided to adjust the weights in 
the 2 matrix to try to gain better control over the pitch, 
and to increase the pitch error gains. The weights that 
were adjusted were those that related pitch errors, elements 
Q(ll,ll), Q(ll,16), Q(16,ll), Q(16,16). 

When these elements were increased by a factor of 1000 
the pitch error gains increased and a tighter control over 
pitch was achieved as shown in Figures 6.13 to 6.17. 

Comparing Figures 6 . 7 and 6 . 17 shows the tighter control 
gained over the pitch. With the tighter control gained in 
pitch a slight degradation in depth control was observed. 
Figures 6.14 and 6.15 show a small overshoot in ordered 
depth for the vehicle indicating the loosening of the depth 
control modes. 
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Figure 6.13 Tighter Pitch Control Weighting Stern Plane Angle 
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Figure 6.14 Tighter Pitch Control Weighting Dive Profile 
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Figure 6.15 Tighter Pitch Control Weighting Depth Vs Time 
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Figure 6.16 Tighter Pitch Control Weighting Bow Plane Angle 
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Figure 6.17 Tighter Pitch Control Weighting Pitch Angle 



F. FURTHER PITCH CONTROL WEIGHTING 

- The Q matrix pitch elements were further increased by a 
factor of 10 to observe the correlation between depth and 
pitch control. Again Figures 6.18 to 6.22 show a sluggish 
response in depth control while gaining a much tighter 
control over pitch. However, in this case the command for 
the bow planes have exceeded their physical limits and are 
commanded to an unreasonable amount as shown in Figure 6.21. 

As the linear controller can command a control action 
greater than the physical limits of the vehicle, when poor 
weights are selected, logic was added to the DSL code to 
limit the plane action to plus or minus .6 radians on the 
bow and stern planes. 

Although the increased weights in the Q matrix gave a 
better pitch response, its effects on tracking control were 
undesirable. For this reason, and for all subsequent 
numerical experiments, it was decided to use the gains 
originally calculated in the baseline run and the original Q 
matrix weights. 

G. EFFECT OF SPEED MISMATCH MODEL/VEHICLE 

The major issue of control robustness relates to the 
seriousness of speed mismatch between model and AUV. So the 
next task was to test this controller over a range of 
vehicle speeds, 3, 12 and 30 feet/sec. 

Using the controller and model based on a speed of 6 
ft/sec, the simulation was run using a vehicle speed of 12 
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Figure 6.18 Further Pitch Control Weighting Stern Plane Angle 
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Figure 6.19 Further Pitch Control Weighting Dive Profile 
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Figure 6.20 Further Pitch Control Weighting Depth Vs Time 
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Figure 6.21 Further Pitch Control Weighting Bow Plane Angle 
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Figure 6.22 Further Pitch Control Weighting Pitch Angle 



ft/sec. Figures 6.23 to 6.27 show very good tracking 
ability even though the vehicle was going twice the speed. 
Figure 6.24 shows that the vehicle went twice as far as the 
model to reach the same depth, due primarily to the vehicle 
speed being double that of the model. Figure 6.27 shows the 
compensation in pitch angle to achieve desired depth. If 
the controller was tighter in pitch it would have followed 
closer in this figure but in Figure 6.25 the accurate 
trajectory tracking would be lost. Again this goes back to 
the type of control needed and adjusting of the weights in 
the 2 and R matrix to generate satisfactory control gains. 

The next test of the controller was an attempt to run 
the vehicle at a speed of 3 ft/sec which is very slow and 
yet try to use a model speed of 6 ft/sec. The primary 
motivation was to see if one set of gains and one model 
could be used for all maneuvers, rather than recalculating 
gains every time the vehicle changed speeds; a test of 
robustness in the controller. When the vehicle was operated 
at 3 ft/sec the vehicle started out lagging the model and 
then control errors grew while the controller commanded more 
and more action. But, since the vehicle was much slower 
than the model ordered, depth and path following could not 
be achieved. 

To alleviate this problem, gains were recalculated and 
the model was run at 3 ft/sec (Figures 6.28 to 6.32) when 
excellent tracking was restored. However, at the slower 
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Figure 6.23 Speed Mismatch 12 Ft/Sec Stern Plane Angle 




(XJ) SOdZ 



85 



Figure 6.24 Speed Mismatch 12 Ft/Sec Dive Profile 
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Figure 6.25 Speed Mismatch 12 Ft/Sec Depth Vs Time 
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Figure 6.26 Speed Mismatch 12 Ft/Sec Bow Plane Angle 
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Figure 6.27 Speed Mismatch 12 Ft/Sec Pitch Angle 
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Figure 6.28 Vehicle and Model 3 Ft/Sec Stern Plane Angle 
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Figure 6.29 Vehicle and Model 3 Ft/Sec Dive Profile 
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Figure 6.30 Vehicle and Model 3 Ft/Sec Depth Vs finis 
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Figure 6.31 Vehicle and Model 3 Ft/Sec Bow Plane Angle 
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Figure 6.32 Vehicle and Model 3 Ft/Sec Pitch Angle 



speed (only 1.75 knots) the dive maneuver algorithm used was 
not. sufficient to maintain pitch during a diving trajectory. 
The buoyant moment overcame the hydrodynamic moment from 
control surfaces and ordered depth was not achieved. This 
behavior, however, is not characteristic of the controller 
but rather the maneuver logic, and as far as the controller 
is concerned it was able to follow the model rather nicely. 

Since the methodology here was to design a controller 
that was robust enough to handle a wide variety of reflexive 
type maneuvers over a range of speeds it is most likely that 
the vehicle will be traveling at much greater speeds when 
these maneuvers are executed. For this reason, another 
simulation run was made. Again the control weights and 
gains used were as per the baseline case of 6 ft/sec. The 
model was also at 6 ft/sec and this time the vehicle was at 
3 0 ft/sec. Figures 6.33 to 6.37 show once again excellent 
tracking control, and like the 12 ft/sec case tight pitch 
control was eased in favor of accurate depth and trajectory 
control, which is desirable not to have the vehicle 
violently pitching during a maneuver which may result in 
vehicle equipment damage. 

H. EIGENVALUES — LINEAR MODEL 

The following presents a table of the eigenvalues of the 
baseline model at 6 ft/sec forward speed together with the 
closed loop eigenvalues found using the baseline weights. 
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Figure 6.33 Speed Mismatch 30 Ft/Sec Stern Plane Angle 
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Figure 6.34 Speed Mismatch 30 Ft/Sec Dive Profile 
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Figure 6.35 Speed Mismatch 30 Ft/Sec Depth Vs Time 
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Figure 6.36 Speed Mismatch 30 Ft/Sec Bow Plane Angle 
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Figure 6.37 Speed Mismatch 30 Ft/Sec Pitch Angle 



TABLE 4 



Eigenvalues 

1 

2 

3 

4 

5 

6 

7 

8 
9 

10 

11 

12 

13 

14 

15 

16 

17 

18 
19 



OPEN LOOP EIGENVALUES 

Real Part 

-0. 1600E-02 

-0. 1500E-02 

-0.1700E-02 

-0.1800E-02 

-0.2100E-02 

“0. lOOOE-03 

-0. 1663E+01 

-0.6579E+00 

“0.9553E+00 

“0.1122E+00 

-0. 1122E+00 

-0.3909E+00 

-0.1603E-01 

-0.9553E+00 

-0.3908E+00 

-0.1423E-01 

-0. 3000E-03 

-0.4000E-03 

-0.5000E-03 



Imaginary Part 
0. OOOOE+00 
O.OOOOE+00 
O.OOOOE+00 
O.OOOOE+00 
O.OOOOE+00 
0. OOOOE+00 
O.OOOOE+00 
O.OOOOE+00 
O.OOOOE+00 
0.7003E-02 
-0.7003E-02 
0. OOOOE+00 
0. OOOOE+00 
0. OOOOE+00 
O.OOOOE+00 
0. OOOOE+00 
O.OOOOE+00 
0. OOOOE+00 
0. OOOOE+00 
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TABLE 5 



Eiaenvalues 


CLOSED LOOP EIGENVALUES 
Real Part 


Imaainarv Part 


1 


-0.1600E-02 


O.OOOOE+00 


2 


-0.1500E-02 


O.OOOOE+OO 


3 


-0.1700E-02 


0. OOOOE+00 


4 


-0.2100E-02 


O.OOOOE+OO 


5 


-0.1663E+01 


O.OOOOE+OO - 


6 


-0.9888E+00 


O.OOOOE+OO 


7 


-0.3456E+00 


0.4402E+00 


8 


-0. 3454E+00 


-0.4402E+00 


9 


-0.6579E+00 


O.OOOOE+OO 


10 


-0.4868E+00 


O.OOOOE+OO 


11 


-0.9553E+00 


O.OOOOE+OO 


12 


-0.1122E+00 


0.7003E-02 


13 


-0.1122E+00 


-0.7003E-02 


14 


-0.3908E+00 


O.OOOOE+OO 


15 


-0.1423E-01 


O.OOOOE+OO 


16 


-O.lOOOE-03 


O.OOOOE+OO 


17 


-0.3000E-03 


O.OOOOE+OO 


18 


-0.4000E-03 


O.OOOOE+OO 


19 


-0.5000E-03 


O.OOOOE+OO 



101 



VII. SUMMARY. CONCLUSIONS. AND RECOMMENDATIONS 



A. SUMMARY 

This thesis presents a study of Model Reference Controls 
for an Autonomous Underwater Vehicle. The approach to the 
design and testing of a model following autopilot included: 

1. Selection of a suitable submersible was selected, one 

that displayed many attributes for potential AUV 
missions. One in which all the hydrodynamic 

characteristics were well studied and data were 
obtainable. 

2. The tailoring of the existing equations of motion to 
gain control over all six degrees of freedom. 

3. The development of a linearized model and programming 
the linearized and non-linear models for simulation 
using Dynamic Simulation Language (DSL) . 

4. The development of a 19 state controller for dive 
plane maneuvers. Maneuvers that could be termed 
reflexive. 

5. The development of logic for a command generation 
system for a dive maneuver. 

6. Observation of the effects on the control’ gains by 
varying the weights in the minimizing function J. 

7. The testing of the command generation logic and the 
controller over a wide range of speeds using only one 
set of calculated gains based on one speed of 6 
ft/sec. 



B. CONCLUSIONS 

In this study, a methodology was developed to the design 
of a model following autopilot that could be used in an 
Autonomous Underwater Vehicle. A 19 state controller was 
designed for automatic control of maneuvers in the dive 
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plane. This controller displayed excellent trajectory 
following characteristics and exhibited a high degree of 
robustness over a five to one speed range. 

The model following autopilot was designed to follow 
trajectories generated from a preprogrammed maneuver 
algorithm. This maneuver logic proved to be workable and 
could easily be developed for a wide variety of maneuvers to 
be stored on-board in a computer. 

In this study maneuver logic was created for one such 
maneuver, a dive maneuver, and was followed by the designed 
autopilot. The algorithm used for the dive maneuver was 
crude but sufficiently proved that the design methods are 
sound. 

Some difficulties in perfect trajectory following occur 
because of speed mismatch between model and vehicle, and an 
improvement in modelling speed loss during maneuvers would 
be worthwhile. 

C . RECOMMENDATIONS 

Because the concept of Autonomous Underwater Vehicles is 
fresh and significant progress has been made in the 
computational abilities of modern computers, a wide 
diversification of technological avenues need to be 
explored. Specific to this study the following 

recommendations are presented. 

1. An implementation of the full 3 0 state dynamically 
coupled controller in an AUV should be the ultimate 
goal of this project. In particular, the influence 
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for forward speed changes should be reflected in the 
maneuver command generation model for greater control 
accuracy. 

2 . Parallel efforts should be carried forward with the 
development of many maneuver algorithms that could be 
stored in the AUV's "bag” of maneuvers. 

3. Although this controller was designed specifically for 
the control of an AUV with an unusual geometry, it can 
and should be tested on underwater vehicles with other 
geometries . 
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APPENDIX A 



SIX-DEGREE-OF-FREEDOM EQUATIONS OF MOTION 
AND EULER ANGLE RATES 



SURGE EQUATION OF MOTION 

• • • 

m[u - vr + wq - Xc(q2 + r^) + yc(pq - r) + ZQ(pr + q) ] 



= e ? 4 



5-^[Xpp + Xcja + Xj-r + Xpj- pr] 



+ I u + X^q wq + Xvp vp + Xy^ vr 

+ uq(Xq5g "Sg + Xq5j^/2 “^bp "*■ ^q6b/2 ‘^j^g 

+ Xr6r ur6r] 

^ J-^[Xyy + X^^ + Xy(Sj- UV 6r 

I I 

+ uw(X^gg 6s X^5]3/2 ‘Sbs 
+ ^\ldD/2 bp^ 

+ ^^(X6s6s s X6b6b/2 ^bp ■*■ X6b5b/2 *^bs 
+ ^ 6r 6r '^r ) 3 

- (W-B) sin 0 + f 5-3 Xq5sj, uq6g e(n) 

+ ■j5,3[x^5gj^ uw6 g + X^ggfj *^s3 ^ (u) 



"qq 



'pr 



+ — 5,^ X 

2 -^prop 



(A-1) 
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SWAY EQUATION OF MOTION 
• • 
m[v + ur - wp + X( 3 (pq + r) - YqCp^ + r^) 

+ ZG(q^ - p) ] 



n A • ' * 

*“ *^5/ C^p P ^ ^P9 ^ ^qr 5^] 

I 

+ f V + Yp up + Yjl ur + Yvq vq 

+ wp + Yvr wr] 



+ I il^C^v uv + Y^ vw + Ygj- u2 6j-] 

X 

nose 

- ^. / [Coy h(x) (v+xr)2 

^tail 

+ Cdz b(x) (w-xq)2] dx 

o f 

+ (W-B) COS 0 sin 4) 
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(A-2) 



K)|-o 



HEAVE EQUATION OF MOTION 



m[w + uq + vp + XQ(pr - q) + Yciqr + p) 

- zg(p^ + q^) ] 

I • ^ 

q + Zpp p2 + Zpj- pr + Zj-j- r2] 

no'* ' ' 

+ ^ Sr[Z^ w + Z(^ uq + Zvp vp + Z^j- vr] 

+ ^ [Z^ uw + Zw 

+ u2(Z5s + Z6b/2 "^bs + ^Sh/2 *^bp] 
^nose 

- f / [Coy h(x) (v+xr)2 

^tail 

+ Cd 2 b(x)(w-xq)2] dx 

+ (W-B) COS 8 cos (f) 

+ f Zqn uq e(n) 

I » 

+ 2 uw + Z< 5 sn u2 6g] e (n) 



(A-3) 
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ROLL EQUATION OF MOTION 



• • 

P “ ^y) ^xy(P^ “ Q[) “ ^yz(^^ “ 

• • • 

- Ixz(P‘3+^) + ro[yc(w - uq + vP) - Zq(v + ur - wp)] 

I I 

f ^^[Kp p + KJ- r + Kpq pq + Kgj. qr] 

P ' . 

+ 2-^ ^ V + Kp up + Kj- ur + K^q vq 

I I 

^wp ^wr 1 

+ 2 ^^[Kv UV + Kvw VW + u2(K5b/2 *^bp + ^6b/2 "^s) 3 

+ (YqVI - ygB) COS 6 COS (|) - (ZqW - ZgB) cos 0 sin <i> 

+ |- £4 Kpn up s (n) 

+ — 5, ^ TC 
+ 2 ^ u i^prop 



(A-4) 
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PITCH EQUATION OF MOTION 



• • • 

ly q 4* (Ix “ ^yz (P^ ” 

• • 

+ 1x2 " i^[Xg(w - uq + vp) - Zg(u - vr + wq] 

1*1 I I 

= I ^^[Mq q + Mpp p2 + Mpj. pr + Mj-r ^2] 

Da'’ ' ' ' 

+ ^ w + Mq uq + M^p vp + Mvr vr] 

+ I S?[kIj uw + v2 

+ + ^sh/2 ^hp + ^6h/2 *^bs) ] 

X 

nose 

+ f / [Coy h(x) (v+xr)2 

^tail 

+ Cd 2 b(x) (w-xq)2] X dx 

- (XqW - XgB) COS 6 cos 4) - (ZqW - ZgB) sin 6 
+ j Mqn uq e(n) 

+ ^ £.^[Mwn + M6sn ^2 6g] e(n) 
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K>|*D 



YAW EQUATION OF MOTION 



Iz ^ + (^y " Ix)Pq ~ Ixy(P^-<3^) ” Iyz(P^+q) 

• • 

+ Ixz(qr-p) + m[XG(v + ur - wp) - yc(u - vr 

I 

^^[Np p + Nr r + Npg pq + Ngj- qr] 

I 

+ ^ V + Np up + Nr ur + N^g vq 

I I 

+ wp .+ N^r wr] 

+ I A^[Nv uv + N^ vw + N^r 

- f / [Cpy h(x) (v+xr )2 

^tail 

+ Cd 2 b(x) (w-xq)2] 

cf 

+ (xqW - xgB) cos 6 sin<}) + (ygW - ygB) sin 

+ ^ 5.3 u2 N ' 

2 ^ u iMprop 



+ wq)] 



xdx 
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Euler Angle Rates 



4) = P + g sin (J) tan 6 + r cos tan 6 



6 = q cos (j) - r sin (p 

^ — q 



sin 4> ^ cos 4> 
cos ^ cos ^ 



Inertial Position Rates 



^0 


= ^co + ^ 


cos Ip 


cos e 
















+ v[cos 


tp sin 


e 


sin 


4) 


- 


sin 




cos 


4>] 




+ w[cos 


tp sin 


e 


cos 


4> 


+ 


sin 




sin 


4>] 


^0 


= Vco + u 


sin ip 


cos 0 
















+ v[sin 


jp sin 


e 


sin 


<P 


+ 


cos 




cos 


<P] 




+ w[sin 


ip sin 


e 


cos 


<P 


- 


cos 




sin 


<P] 



Zq " '"cO " sin e 

+ V cos 6 sin \p 
+ w cos e cos (p 



Crossflow Velocity 



u .(x) = [(V + xr)2 + (w - xq)2]l/2 
cr 
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APPENDIX B 

DSL LISTING FOR AUV SIMULATION 



TITLE AUTONOMOUS UNDERWATER VEHICLE (AUV) SIMULATION 
D COMMON/ BLOCKl/ MMINV(6,6), MM(6,6), AA(12,12), BB(6,6) 

D C0MM0N/BL0CK2/ B(6,6) ,A(12 ,12) , UMOD(6) ,GKK(6,21) 

D COMMON/BLOCK3/ F(12) , FP(6), UCF(4) 

D COMMON/ BL0CK4/ G4(4) ,GK4(4) ,BR(4) ,HH(4) 

D COMMON/BLOCK5/ XDOT(12) ,XDOTX(12) , XDOTU(6) 

FIXED N,IA,IDGT,IER,LAST,J,K,M,JJ,KK,I 
INTEGER 

ARRAY WKAREA(54), X(12) 

•k 

k 



CONST 

* 






* LONGITUDINAL 

* 


HYDRODYNAMIC 


COEFFICIENTS 


CONST XPP = 


,XQQ = 


,XRR = 


XUDOT= 


,XWQ = 


,XVP = 


XQDS= 
JWW = 


,XQDB= 

,XVDR= 


,XRDR= 

,XWDS= 


XDSDS= 


,XDBDB= 


,XDRDR= 


XWDSN= 

* 


, XDSDSN= 





,XPR = 
,XVR = 
,XW = 
,XWDB= 
,XQDSN= 



* 

* 



LATERAL HYDRODYNAMIC COEFFICIENTS 



CONST 


YPDOT= 


,YRDOT= 


,YPQ = 


,YQR 


= 




YVDOT= 


,YP = 


,YR = 


,YVQ 






YWP = 


,YWR = 


,YV = 


,YVW 


= 


k 


YDR = 


,CDY = 








k 

k 


NORMAL 


HYDRODYNAMIC COEFFICIENTS 








CONST 


ZQDOT= 


,ZPP = 


,ZPR = 


,ZRR 


= 




ZWDOT= 


,ZQ = 


,ZVP = 


,ZVR 


= 




ZW = 


,ZVV = 


,ZDS = 


,ZDB 


= 


k 


ZQN = 


,ZWN = 


, ZDSN= 


,CDZ 




k 

k 


ROLL HYDRODYNAMIC COEFFITIENTS 








CONST 


KPDOT= 


, KRDOT= 


,KPQ = 


,KQR 






KVDOT= 


, KP = 


,KR = 


,KVQ= 






KWP = 


, KWR = 


,KV = 


,KVW 


= 


k 


KPN = 


, KDB = 








k 

k 


PITCH HYDRODYNAMIC COEFFICIENTS 








CONST 


MQDOT= 


, MPP = 


,MPR = 


,MRR 


= 




MWDOT= 


, MO = 


,MVP = 


,MVR 


= 




MW = 


, MVV = 


,MDS = 


,MDB 


= 


k 


MQN = 


, MWN = 


,MDSN = 






k 

k 


YAW HYDRODYNAMIC COEFFICIENTS 








CONST 


NPDOT= 


, NRDOT= 


,NPQ = 


,NQR 


= 




NVDOT= 


, NP = 


,NR = 


,NVQ 


= 




NWP = 


, NWR = 


,NV = 


,NVW 


= 




NDR = 










k 

k 


MASS CHARACTERISTICS OF THE FLOODED auv 






CONST 


WEIGHT 


= , BOY = 


,VOL = 


,XG = 






YG = 


, ZG = 


,XB = 


,ZB = 
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yh yh >** ^ >*• 



IX = 


, lY = 


,IZ = 


,IXZ = 


lYZ = 


, IXY = 


,YB = 




L = 


, RHO = 


,G = 


,NU = 


AO = 


,KPROP = 


,NPROP = 


, XITEST 



INPUT INITIAL CONDITIONS HERE IF REQUIRED 



INITIALIZE ALL MATRICES AND ARRAYS TO ZERO 
N = 6 

DO 2 J = 1,N 
JJ= J+N 
DO 1 K = 1,N 
KK= K+N 
KKK= KK + N 
MMINV(J,K) = 0.0 
X(J) = 0.0 
X(JJ) = 0.0 
XDOT(J) = 0.0 
XDOT(JJ) = 0.0 
XDOTX(J) =0.0 
XDOTX(JJ) = 0.0 
XDOTU(J) =0.0 
UMOD(J) =0.0 
MM(J,K) =0.0 
BB(J,K) = 0.0 
B(J,K) = 0.0 
AA(J,K)= 0.0 
AA(JJ,KK)= 0.0 
AA(J,KK)= 0.0 
AA(JJ,K)= 0.0 
A(J,KK)= 0.0 
A(JJ,K) = 0.0 
A(J,K) = 0.0 
A(JJ,KK)= 0.0 
GKK(J,K)= 0.0 
GKK(J,KK)=0.0 
GKK(J,KKK)=0.0 
CONTINUE 
CONTINUE 

INPUT THE LINEARIZATION POINT PARAMETERS 

UO =6.0 
VO = 0.0 
WO = 0.0 
PO = 0.0 
QO = 0.0 
RO = 0.0 
PHIO = 0.0 
THETAO =0.0 
PSIO = 0.0 
SUM = 0.0 
JFLAG = 0 
I FLAG = 0 
KFLAG = 0 
ZORD = 100.0 

INPUT THE MODEL STATES INITIAL CONDITIONS 

UM = 6.0 
VM = 0.0 
WM = 0.0 
PM = 0.0 
QM = 0.0 
RM = 0.0 
XPOSM =0.0 
YPOSM =0.0 
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jf 5f ^ jf >!• :f ^ if 



ZPOSM =0.0 
PHIM =0.0 
THETAM =0.0 
PSIM = 0.0 



INPUT THE VEHICLE INITIAL CONDITIONS 



U = 6.0 
V = 0.0 
W = 0.0 
P = 0.0 
Q = 0.0 
R = 0.0 
XPOS =0.0 
YPOS =0.0 
ZPOS =0.0 
PSI = 0.0 
THETA =0.0 
PHI = 0.0 

INITIALIZE THE CONTROLS 

DELBOY= 0.0 
DBS= 0.0 
DBP= 0.0 
DS = 0.0 
DR = 0.0 
RPM = 250.00 
LATYAW =0.0 
NORPIT =0.0 

DEFINE LENGTH FRACTIONS FOR GAUSS QUADUTURE TERMS 

G4(l) = 0.069431844 
G4(2) = 0.330009478 
G4(3) = 0.669990521 
G4(4) = 0.930568155 

DEFINE WEIGHT FRACTIONS FOR GAUSS QUADUTURE TERMS 

GK4(1) = 0.1739274225687 
GK4(2) = 0.3260725774312 
GK4(3) = 0.3260725774312 
GK4(4) = 0.1739274225687 

DEFINE THE BREADTH BB AND HEIGHT HH TERMS FOR THE INTEGRATION 

BR(1) = 75.7/12 
BR(2) = 75.7/12 
BR(3) = 75.7/12 
BR(4) = 55.08/12 

HH(1) = 16.38/12 
HH(2) = 31.85/12 
HH(3) = 31.85/12 
HH(4) = 23.76/12 

MASS = WEIGHT/G 

DIVAMP = DEGSTN*0. 0174532925 
RUDAMP = DEGRUD*0. 0174532925 



THE LINEAR PROPULSION MODEL ' 



ETA = 0. 012*500. O/UO 
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5*- if ifififififif 



ETA = 1.0 
RE = U0*L/NU 

CDO = .00385 + (1.296E-17)*(RE - 1.2E7)**2 

CT = 0.008*L**2^ETA*ABS(ETA)/(A0) 

CTl = 0.008*L**2/(A0) 

EPS = -1.0+(SQRT(CT+1.0)-1.0)/(SQRT(CT1+1.0)-1.0) 
XPROP = CDO*(ETA*ABS(ETA) - 1.0) 

MASS = WEIGHT/G 

DIVAMP = DEGSTN*0. 0174532925 

RUDAMP = DEGRUD*0. 0174532925 



CALCULATE THE MASS MATRIX 



MM(1,1) 

MM(1,5 

MM(1,6] 

MM(2,2) 

MM(2,4 

MM(2,6) 

MM(3,3] 

MM(3,4 

MM(3,5; 

MM(4,2] 

MM(4,3 

im(4,4 

MM(4,5 

MM(4,6j 



MASS -((RHO/2)*(L**3)*XUDOT) 

MASS*ZG 

-MASS*YG 

MASS -((RH0/2)*(L**3)*YVD0T) 
-MASS*ZG -((RH0/2)*(L**4)*YPD0T) 
MASS*XG - ((RH0/2)*(L**4)*YRD0T) 

MASS - ((RH0/2)*(L**3)*ZWD0T) 
MASS*YG 

-MASS^XG -((RH0/2)*(L**4)*ZQD0T) 

-MASS*ZG - ((RH0/2)*(L**4)*KVD0T) 
MASS*YG 

IX - ((RHO/2)*(L**5)*KPDOT) 

-IXY 

-IXZ -((RHO/2)*(L**5)*KRDOT) 



MM(5,1) = MASS*ZG 

MM(5,3) = -MASS*XG -( (RH0/2)*(L**4)*MWD0T) 
MM(5,4) = -IXY 

MM(5,5) = lY -( (RHO/2)*(L**5)*MQDOT) 
MM(5,6) = -lYZ 



-MASS*YG 

MASS*XG -((RH0/2)*(L**4)*NVD0T) 
-IXZ - ((RHO/2)*(L’^*5)*NPDOT) 
-lYZ 

MM(6,6) = IZ - ( (RHO/2)*(L**5)*NRDOT) 



MM(6,i; 

MM(6,2 

MM(6,4 

MM(6,5 



LAST = N*N+3*N 
DO 20 M = 1,LAST 
WKAREA(M) =0.0 
CONTINUE 

lER = 0 
lA = 6 
IDGT = 4 

WRITE( 8,400)((MM(I,J), J = 1,6), I = 1,6) 
CALL LINV2F (MM , N , I A , MMINV , IDGT , WKAREA , lER ) 

WRITE( 8,400)((MMINV(I,J), J = 1,6), I = 1,6) 

*00 FORMAT (6E 12. 4) 

* 

•k 

* CALCULATE THE A MATRIX FOR THE LINEAR MODEL 



A(l,l) = RHO/2*L**3*(XQDS*DS*QO+XQDB/2*DBP*QO+XRDR*RO*DR)+. . . 

RHO/2*L**2*(XVDR*VO*DR+XWDS*DS*WO+XWDB/2*DBP*WO + .. 
2*U0*(XDSDS^DS**2 + XDBDB/2*DBP**2 + XDRDR*DR**2) )+ 
RHO/2^L**3*XQDSN*QO*DS*EPS+RHO/2*L**2*(XWDSN*WO*DS + 
2*XDSDSN*UO*DS**2)*EPS+RHO*L**2*UO*XPROP+RHO/2*L**3* 
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XQDB/2*DBS*QO+RHO/2*L**2*XWDB/2*DBS*WO+RHO*L**2*UO*. . . 
XDBDB/2*DBS**2 

A(l,2) = MASS*RO+RHO/2*L**3*{XVP*PO+ XVR*RO) + RH0/2*L**2* ... 
(2*XVV*V0 + XVDR^UO’^DR) 

A(l,3) = -MASS*QO + RHO/2*L**3*(XWO*QO)+RHO/2*L**2*(2*XtW*WO+. . . 

XWDS*DS*UO+(XWDB/2*DBP+XWDB/2*DBS)*UO +XVJDSN*UO*DS*EPS) 
A(l,4) = -MASS*YG*QO-MASS*ZG*RO+ RHO/2*L**4*(2*XPP*PO+XPR*RO ) . . . 
+ RHO/2*L**3*(XVP*VO) 

A(l,5) = -MASS*W0+2*MASS*XG*Q0 -MASS*YG*PO+RHO/2*L**4*2*XQQ*QO . . 

+RHO/2*L**3*(XWQ*WO+XQDS*DS*UO+XQDB/2*DBP*UO)+RHO/2^ . . 
L**3*XQDSN*UO*DS*EPS+RHO/2*L**3*XQDB/2*DBS*UO 
A(l,6) = MASS*VO+2*MASS*XG*RO-MASS*ZG*PO+RHO/2*L**4*(2*XRR*RO. . . 

+ XPR*PO) + RHO/2*L**3*(XVR*VO + XRDR*UO*DR) 

A(l,ll)= -{WEIGHT - BOY)*COS{THETAO) 

A(2,l) = -MASS*RO+RHO/2*L**3*(YP*PO+YR*RO)+RHO/2*L**2*(YV*VO+. . . 
2*YDR*UO*DR) 

A(2,2) = RHO/2*L**3*YVQ*QO+RHO/2*L**2*(YV*UO+YVW*WO) 

A(2,3) = MASS*PO+ RHO/2*L**3*(YWP*PO+YWR*RO)+RHO/2*L**2*YVW*VO 
A(2,4) = MASS*WO-MASS*XG*QO+2’'MASS*YG*PO+RHO/2*L**4*YPQ*QO+. . . 
RHO/2*L**3*(YP*UO+ YWP*WO) 

A(2,5) = -MASS*XG*PO-MASS*ZG*RO+RHO/2*L**4*(YPQ*PO+YQR*RO) +... 
RHO/2*L**3*YVQ*VO 

A(2,6) = -MASS*UO+2*MA§S*YG*RO-MASS*ZG*QO+RHO/2*L**4*YQR*QO +... 

RHO/2*L**3*(YR*UO + YWR*WO) 

A(2,10)= (WEIGHT - BOY)*COS(THETAO)*COS(PHIO) 

A(2,ll)= -(WEIGHT - BOY)*SIN(THETAO)*SIN(PHIO) 

A(3,l) = MASS*QO+RHO/2*L**3*ZQ*QO+RHO/2*L**2*(ZW*WO+2*UO*ZDS*DS. 

+2*UO*ZDB/2*DBP+(ZWN*WO+2*ZDSN*UO*DS)*EPSHRHO/2*L**3*. 
ZQN*QO*EPS+ RHO/2*L**2*2*UO*ZDB/2*DBS 
A(3,2) = -MAS§*PO+RHO/2*L**3*(ZVP*PO+ZVR*RO)+RHO/2*L**2*2*ZVV*VO 
A (3, 3) = RHO/2*L**2*(ZW*UO + ZWN*UO*EPS) 

A(3,4) = -MASS*VO-MASS*XG*RO+2*MASS*ZG*PO+ RHO/2*L**4*(2*ZPP* . . . 

PO + ZPR*RO) + RHO/2*L**3*ZVP*VO 
A(3,5) = MASS*UO - MASS*YG*RO+2*MASS*ZG*QO+RHO/2*L**3*ZQ*UO +... 
RHO/2*L**3*ZQN*UO*EPS 

A(3,6) =-MASS*XG*PO-MASS*YG*QO+RHO/2*L**4*(ZPR*PO+2*ZRR*RO)+. . . 
RHO/2*L**3*ZVR*VO 

A(3,10)= -(WEIGHT - BOY)*COS(THETAO)*SIN(PHIO) 

A(3,ll)= -(WEIGHT - BOY)*SIN(THETAO)*COS(PHIO) 

A(4,l) = MASS*YG*QO + MASS*ZG*RO + RHO/2*L**4*(KP*PO + ... 

KR*RO)+RHO/2*L**3*(KV*VO+2*UO*(KDB/2*DBP-KDB/2*DBS))+. . 
RHO/2*L**3*UO*KPROP+ RHO/2*L**4*KPN*PO*EPS 
A(4,2) = -MASS*YG*PO + RHO/2*L**4*KVQ*QO + RHO/2*L**2*(KV*UO . . . 

+ KVW*WO) 

A(4,3) = -MASS*ZG*PO + RHO/2*L**4*(KWP*PO + KWR*RO) + ... 
RHO/2*L**3*KVW*VO 

A(4,4) = -IXY*RO + IXZ*QO - MASS*YG*VO - MASS*ZG*WO + ... 

RHO/2*L**5*KPQ*QO + RHO/2*L**4*(KP*UO+KWP*WO) 

A(4,5) = -IZ*RO + IY*RO + 2*IYZ*QO + IXZ’^PO + MASS*YG^O +... 

RHO/2*L**5*(KPQ*PO + KQR*RO) + RHO/2*L**4*KVQ*VO 
A(4,6) = -IZ*QO + IY*QO - 2*IYZ*R0 + MASS*ZG*UO + ... 

RHO/2*L**5*KQR*QO + RHO/2*L**4*(KR*UO+KWR*WO) 

A(4,io)= -(yg*weight-yb*boy)*cos(thetao)*sin(phio) . . . 

-(ZG*WEIGHT-ZB*BOY)*COS(THETAO)*COS(PHIO) 

A(4,ii)= -(yg*weight-yb*boy)*sin(thetao)*cos(phio) . . . 

+(ZG*WEIGHT-ZB*BOY)*SIN(THETAO)*SIN(PHIO) 

A(5,l) = -MASS*XG*QO + RHO/2*L**4*MQ*QO + RHO/2*L**3*MW*WO +... 

RHO/2*L**3*UO*(MDS*DS+MDB/2*DBP) + RHO/2*L**4*MQN*QO* . . 
EPS + RHO/2*L*^3*(MWN*WO + 2*MDSN*U0*DS)*EPS+. . . 
RHO/2*L**3*UO*MDB/2*DBS 

A(5,2) = MASS*XG*PO + MASS*ZG*RO + RHO/2*L**4*(MVP*PO + ... 
MVR*RO) + RHO*L**3*MVV*VO 

A(5,3) = -MASS*ZG*QO + RHO/2*.L**3*MW*UO + RHO/2*L**3*IWN*UO*EPS 
A(5,4) = -IX*RO + IZ*RO - IYZ*QO - 2*IXZ*PO + MASS*XG*VO + ... 

RHO/2*L**5*(2*MPP*PO + MPR*RO) + RHO/2*L**4*MVP*VO 
A(5,5) = IXY*RO -lYZ’^PO - MASS*XG*UO -MASS*ZG*WO + RHO/2*... 
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L**4*MQ*U0 + RHO/2*L**4*MQN*UO*EPS 
A(5,6) = -IX*PO + IZ*PO + IXY*QO + 2*IXZ*R0 + MASS*ZG*VO +. . . 

RHO/2*L**5*(MPR*PO+2*MRR*RO)+RHO/2*L**4*MVR*VO 
A(5,10)= (XG*WEIGHT-XB*BOY)*COS(THETAO)*SIN(PHIO) 

A(5,ll)= (XG*WEIGHT-XB*BOY)*SIN(THETAO)*COS(PHIO) - ... 
(ZG*WEIGHT-ZB*BOY) *COS (THETAO ) 

A(6,l) = -MASS*XG*RO + RHO/2*L**4*(NP*PO +NR*RO) + RHO/2*... 

L**3*(NV*VO+2*NDR*UO*DR)+RHO*L**3*UO*NPROP 
A(6,2) = -MASS*YG*RO + RHO/2*L**4*NVQ*QO + RHO/2*L**3*(NV*UO+. 
NVW*WO) 

A(6,3) = MASS*XG*PO + MASS*YG*QO + RHO/2*L**4*(NWP*PO+NWR*RO)+ 
RHO/2*L**3*NVW*VO 

A(6,4) = -IY*QO + IX*QO + 2*IXY*P0 +IYZ*RO + MASS*XG*WO+. . . 

RHO/2*L**5*NPQ*Q0 + RHO/2*L**4*(NP*UO+NWP*WO ) 

A(6,5) = -IY*PO + IX*PO - 2*IXY*Q0 - IXZ’*^RO + MASS*YG%0+ . . . 

RHO/2*L**5*(NPQ*PO+NQR*RO) + RHO/2*L**4*NVQ*VO 
A(6,6) = IYZ*PO -IXZ=^QO - MASS*XG*UO -MSS*YG*VO + ... 

RHO/2*L**5*NQR*QO + RHO/2*L**4*(NR*UO +NWR*WO) 
A(6,10)= (XG*WEIGHT-XB*BOY)*COS (THETAO) *COS(PHIO) 

A(6,ll)= -(XG*WEIGHT-XB*BOY)*SIN(THETAO)*SIN(PHIO) +. . . 
(YG*WEIGHT-YB*BOY)*COS (THETAO) 

A(7,l) = COS(PSIO)*COS(THETAO) 

A(7,2) = COS(PSIO)*SIN(THETAO)*SIN(PHIO) - SIN(PSIO)*COS (PHIO) 
A(7,3) = COS(PSIO)*SIN(THETAO)*COS(PHIO) + SIN(PSIO)*SIN(PHIO) 
A(7,10)= VO*COS(PSIO)*SIN(THETAO)*COS(PHIO) + VO*SIN(PSIO)* . . . 

SIN(PHIO) - WO*COS(PSIO)*SIN(THETAO)*SIN(PHIO) + ... 
WO*SIN(PSIO)*COS(PHIO) 

A(7,ll)= -UO*COS(PSIO)*SIN(THETAO) + VO*COS(PSIO)*COS (THETAO)* 
SIN(PHIO) + WO*COS(PSIO)^COS(THETAO)*COS(PHIO) 
A(7,12)= -UO’^SIN(PSIO)*COS(THETAO) - VO*SIN(PSIO)’^SIN(THETAO)* 
SIN(PHIO) - VO*COS(PSIO)*COS(PHIO) - WO*SIN(PSIO)*. . . 
SIN(THETAO)*SIN(PHIO) + WO*COS (PSIO ) *SIN(PHIO) 

A(8,l) = SIN(PSIO)*COS(THETAO) 

MB, 2) = SIN(PSIO)*SIN(THETAO )*SIN(PHI0) + COS(PSIO)*COS(PHIO) 
A(8,3) = sin(psio)*sin(thetao)*cos(phio) - COS (Psio) * sin(phio ) 
A(8,10)= VO*SIN(PSIO)*SIN(THETAO)*COS(PHIO) - VO*COS(PSIO)*. . . 

SIN(PHIO) - WO*SIN(PSIO)*SIN(THETAO)*SIN(PHIO) - ... 
WO*COS(PSIO)*COS(PHIO) 

A(8,ll)= -UO*SIN(PSIO)*SIN(THETAO) + VO*SIN(PSIO)*COS(THETAO)* 
SIN(PHIO) •+ WO*SIM(PSIO)’'COS(THETAO)*COS(PHIO) 
A(8,12)= UO*COS(PSIO)*COS(THETAO) + VO*COS(PSIO)*SIN(THETAO)*. 

SIN(PHIO) - VO*SIN(PSIO)*COS(PHIO) + WO*COS(PSIO)*. . . 
SIN (THETAO )*COS(PHIO) + WO*SIN(PSIO)*SIN(PHIO) 

A(9,l) = -SIN(THETAO) 

MB, 2) = COS(THETAO)*SIN(PHIO) 

A(9,3) = COS ( THETAO )*COS( PHI O) 

A ( 9 , 1 0 ) = VO *COS ( THETAO ) *COS ( PHI 0 ) -WO*COS ( THETAO ) *S IN ( PHI 0 ) 
A(9,ll)= -UO*COS(THETAO)-VO*SIN(THETAO)*SIN(PHIO) -... 

WO*SIN (THETAO )*COS(PHIO) 

A(10,4) = 1.0 

A(10,5) = S IN ( PHI 0)*TAN( THETAO) 

A(10,6) = COS(PHIO)*TAN(THETAO) 

A(10,10)= QO*COS(PHIO)*TAN(THETAO) - RO*SIN(PHIO)*TAN(THETAO) 
A(10,ll)= QO*SIN(PHIO)/COS(THETAO)*1.0/COS(THETAO) + ... 

RO*COS ( PHIO ) /COS (THETAO ) *1 . O/COS (THETAO ) 

A(ll,5) = COS(PHIO) 

A(ll,6) = -SIN(PHIO) 

A(ll,10)= -QO*SIN(PHIO) - RO*COS(PHIO) 

A(12,5) = SIN(PHIO)/COS(THETAO) 

A(12,6) = COS(PHIO)/COS(THETAO) 

A(12,10)= QO*COS (PHIO) /COS (THETAO )-RO*SIN (PHIO) /COS (THETAO) 
A(12,ll)= QO*SIN(PHIO)/COS(THETAO)*TAN(THETAO) + ... 

RO*COS ( PHIO ) /COS (THETAO ) *TAN ( THETAO ) 
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WRITE (10, 200) ((A(I,J),J=1, 12) ,1=1,12) 
CALCULATE THE B MATRIX 



B(l,l) = RHO/2*L**3*XRDR*UO*RO+RHO/2*L**2*(XRDR*UO*VO+UO**2*. . 
2*XDRDR*DR) 

B(l,2) = U0*Q0*XQDB/2 + U0*W0*XWDB/2 + U0**2*XDBDB*DBS 
B(l,3) = U0*Q0*XQDB/2 + U0*W0*XWDB/2 + U0**2*XDBDB*DBP 
B(l,4) = U0*Q0*XQDS + U0*W0*XWDS +UO**2*2*XDSDS*DS+RHO/2*L**3* 
XQDSN*U0*Q0*EPS + RHO/2*L**2*(XWDSN*UO*WO + 2*XDSDSN* 
U0**2*DS)^EPS 

B(l,5) = RHO/2*L*^2*0.12*CD0*0.12*RPM 

B(l,6) = SIN(THETAO) 

B(2,l) = RHO/2*L**2*YDR*UO**2 

B{2,6) = -COS(THETAO)*SIN(PHIO) 

B(3,2) = UO**2*ZDB/2*RHO/2*L**2 

B{3,3) = UO**2*ZDB/2*RHO/2*L**2 

B(3,4) = UO**2*ZDS*RHO/2*L**2 + RHO/2*L**2*ZDSN*UO**2*EPS 
B(3,6) = -COS(THETAO)*COS(PHIO) 

B(4,2) =-RHO/2*L**3*UO**2*KDB/2 
B(4,3) = RHO/2*L**3*UO**2*KDB/2 

B(4,6) = -YB*COS(THETAO)*COS(PHIO) + ZB*COS (THETAO)*SIN(PHIO) 

B(5,2) = RHO/2*L**3*UO**2*MDB/2 

B{5,3) = RH0/2*L**3*U0**2*MDB/2 

B(5,4) = RHO/2*L**3*(UO**2*MDS+MDSN*UO**2*EPS). 

B(5,6) = XB*COS(THETAO)*COS(PHIO) + ZB*SIN(THETAO) 

B(6,l) = RHO/2*L**3*NDR*UO**2 

B(6,6) = -XB*COS(THETAO)*SIN(PHIO) - YB*SIN(THETAO) 

WRITE ( 9,300)((B(I,J),J=1,6) ,1=1,6) 

FORMULATE THE A AND B MATRIX FOR STATE SPACE REPRESENTATION 

MULTIPLY MMINV AND DF/DX 

DO 80 I =1,6 
DO 70 J = 1,6 
SUM =0.0 
DO 60 K = 1,6 

SUM = SUM + MMINV(I,K)*A(K,J) 

CONTINUE 
AA(I,J) = SUM 
CONTINUE 
CONTINUE 



MULTIPLY MMINV AND DF/DZ 



DO 50 I = 1,6 
DO 40 J = 7,12 
SUM = 0.0 

DO 30 K = 1,6 

SUM = SUM + MMINV(I,K)*A(K,J) 
CONTINUE 
AA(I,J) = SUM 
CONTINUE 
CONTINUE 



DO 5 I = 7,12 
DO 6 J = 1,12 
AA(I,J) = A(I,J) 
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CONTINUE 

CONTINUE 



WRITE(10,200)((AA(I,J),J=1,12),I=1,12) 
FORMAT ( 6E12.4) - 



MULTIPLY MMINV AND DF/DU 



DO 110 I = 1,6 
DO 100 J = 1,6 
SUM = 0.0 
DO 90 K = 1,6 

SUM = SUM + MMINV(I,K)*B(K, J) 
CONTINUE 
BB(I,J) = SUM 
CONTINUE 
CONTINUE 



WRITE ( 9,300)((BB(I,J) , J=1 ,6) ,1=1 ,6) 
FORMAT (6E12. 4) 



DO 405 I = 
READ (2,401 
WRITE (3, 401 
FORMAT (3E 20 



1,6 

) GKK(I,Jh J=l,21) 
) GKK I,J , J=l,21 
. 10 ) 



DERIVATIVE 

NOSORT 

* 

"k 

k 

WRITE(8,600) 

FORMAT (14HENTERED DERIV.) 



k 

*00 

* 

* 

* 



CALCULATE BB*U PART OF XDOT = AA*X + BB*U 



DO 10 J = 1,6 
SUM =0.0 
DO 15 K = 1,6 

SUM = SUM + BB(J,K)*UMOD(K) 
15 CONTINUE 

XDOTU(J) = SUM 
10 CONTINUE 

* CALCULATE AA*X 

DO 21 J= 1,12 
SUM =0.0 
DO 25 K = 1,12 

SUM = SUM + AA(J,K)*X(K) 

25 CONTINUE 

XDOTX(J) = SUM 
21 CONTINUE 

* CALCULATE XDOT = AA*X + BB*U 

DO 31 J = 1,6 

XDOT(J) = XDOTX(J) + XDOTU(J) 
31 CONTINUE 

DO 35 J = 7,12 

XDOT(J) = XDOTX(J) 

35 CONTINUE 

* 

UDOTM = XDOT(l) 

VDOTM = XDOT(2) 

WDOTM = XDOT(3) 

PDOTM = XDOT (4) 
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QDOTM = XDOT(5) 

RDOTM = XDOT(6) 

XDOTM = XDOT(7) 

YDOTM = XDOT(8) 

ZDOTM = XDOT(9) 

PHMDOT= JCDOT(lO) 

THETMD= XDOT(ll) 

PSMDOT= XDOT(12) 

WRITE (8, 600) 

INTEGRATE XDOT TO GET THE STATE VECTOR X 

UM =INTGRL(6.0, UDOTM) 

VM= INTGRL(0.0, VDOTM) 

WM= INTGRL(0.0, WDOTM) 

PM= INTGRL(0.0, PDOTM) 

QM= INTGRL(0.0, QDOTM) 

RM= INTGRL(0.0, RDOTM) 

XPOSM = INTGRL(0.0, XDOTM) 

YPOSM = INTGRL(0.0, YDOTM) 

ZPOSM = INTGRL(0.0, ZDOTM) 

PHIM = INTGRL(0.0, PHMDOT) 

THETAM = INTGRL(0.0, THETMD) 

PSIM = INTGRL(0.0, PSMDOT) 



X(l) = UM 
X(2) = VM 
X(3) = WM 
X(4) = PM 
X(5) = QM 
X(6) = RM 
X(7) = XPOSM 
X(8) = YPOSM 
X(9) = ZPOSM 
X(10) = PHIM 
X(ll) = THETAM 
X(12) = PSIM 

ZDEPTH = ZORD - X(9) 
THMANG = X(ll)*57.3 
DRM = UMOD(l) 

DBSM= UMOD(2) 

DBPM= UMOD(3) 

DSM = UMOD(4) 

DRPM= UMOD(5) 

DBOY= UMOD(6) 






DBS = UMOD(2) 

DBP = UMOD(3) 

DS = UM0D(4) 

DBS = -(GKK(2,1)*U + GKK(2,2)*V + GKK(2,3)*W + GKK(2,4)*P +... 

GKK(2,5)*Q + GKK(2,6)*R + GKK(2 , 7 )*XPOS + GKK(2 , 8)*YPOS +... 
GKK(2,9)*ZPOS + GKK(2,10)*PHI + GKK(2 , ll)*THETA + ... 

GKK(2,12)*PSI + GKK(2,13)*WM + GKK(2,14)^QM + GKK(2 , 15)* . . . 
ZPOSM + GKK(2,16)*THETAM + GKK(2 , 17 )*UMOD(2) + GKK(2 , 18)* . . . 
UMOD(3) + GKK(2,19)*UMOD(4)) 

DBP = -(GKK(3,1)*U + GKK(3,2)*V + GKK(3,3)*W + GKK(3,4)*P +... 

GKK(3,5)*Q + GKK(3,6)*R + GKK(3 ,7 )*XPOS + GKK(3 , 8)*YPOS +... 
GKK(3,9)*ZPOS + GKK(3,10)*PHI + GKK(3 , 11 )*THETA + ... 

GKK(3,12)*PSI + GKK(3,13)*WM + GKK(3,14)*QM + GKK(3 , 15)* . . . 
ZPOSM + GKK(3,16)*THETAM + GKK(3 , 17 )*UMOD(2) + GKK{3 , 18)* . . . 
UMOD(3) + GKK(3,19)*UMQD(4)) 

DS = -(GKK(4,1)*U + GKK(4,2)*V + GKK(4,3)*W + GKK(4,4)*P +... 

GKK(4,5)*Q + GKK(4,6)*R + GKK(4 ,7 )*XPOS + GKK(4 ,8)*YP0S +. . . 
GKK(4,9)*ZPOS + GKK(4,10)*PHI + GKK(4, 11)*THETA + ... 
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GKK(4,12)*PSI + GKK(4,13)*WM + GKK(4 
ZPOSM + GKK(4,16)*THETAM + GKK{4,17) 
UMOD(3) + GKK(4,19)*UMOD(4)) 



*QM + GKK(4,15)*.. 
®(2) + GKK(4,18)*. 



PUT IN STERN AND BOW PLANE STOPS 

IF(ABS(DBS) .GT.0.6) THEN 
DBS = 0.6*ABS(DBS)/DBS 
END IF 

IF(ABS(DBP) .GT.0.6) THEN 
DBP = 0.6*ABS(DBP)/DBP 
END IF 

IF{ABS(DS) .GT.0.6) THEN 
DS = 0.6*ABS(DS)/DS 
END IF 



•k 



k 

k 

k 



PROPULSION MODEL 



SIGNU =1.0 

IF (U.LT.0.0) SIGNU = -1.0 
IF (ABS(U) .LT.XITEST) U = XITEST 
SIGNN =1.0 

IF (RPM.LT.0.0) SIGNN = -1.0 
ETA = 0.012*RPM/U 
RE = U*L/NU 

CDO = .00385 + (1.296E-17)*(RE - 1.2E7)**2 

CT = 0.008*L**2’^ETA*ABS(ETA)7(A0) 

CTl = 0.008*L**2/{A0) 

EPS = -1.0+SIGNN/SIGNU*(SQRT(CT+1.0)-1.0)/(SQRT(CT1+1.0)-1.0) 
XPROP = CDO*(ETA*ABS(ETA) - 1.0) 



CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
INTEGRATE USING A 4 TERM GAUSS QUADUTURE 



LATYAW =0.0 
NORPIT =0.0 
DO 500 K = 1,4 

UCF(K) = SQRT((V+G4(K)*R*L)**2 + (W-G4(K)*Q*L)**2) 
IF(UCF(K) .GT.lE-10) THEN 

TERMO = (RHO/2)*{CDY*HH(K)*(V+G4(K)*R*L)**2 +... 

CDZ*BR(K)*(W-G4(K)*Q*L)**2) 

TERMl = TERM0*(V+G4(K)*R*L)/UCF(K) 

TERM2 = TERM0*(W-G4(K)*Q*L)/UCF(K) 

LATYAW = LATYAW + TERMl *GK4 (K) *L 
NORPIT = NORPIT + TERM2*GK4(K)*L 
END IF 

500 CONTINUE 



FORCE EQUATIONS 



surge FORCE 

FP(1) = MASS*V*R - MASS*W*Q + MASS^XG’^Q^^Z + MASS*XG*R**2- . . . 

MASS*YG*P*Q - MASS*ZG*P*R + (RHO/2)*L**4*(XPP*P**2 +... 
XQQ*Q**2 + XRR*R**2 + XPR*P*R) +(RHO/2)*L’^’^3*(XWQ*W*Q +. 
XVP*V*P+XVR*V*R+U*Q* ( XQDS *DS+XQDB/ 2*DBP ) +XRDR=^U*R*DR ) + . . 
(RHO/2)*L**2*(XVV*V*’^2 + XWW*W^*2 + XVDR*U*V*DR + U*W*.. 
(XWDS*DS+XWDB/2*DBP)+U**2*(XDSDS*DS**2+XDBDB/2*DBP**2+. . 
XDRDR*DR**2))- (WEIGHT -BOY)*SIN(THETA) +(RHO/2 )*L**3* .. 
XQDSN*U*Q*DS*EPS+(RHO/2)*L^*2*(XWDSN*U*W^DS+XDSDSN*U**2* 
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DS**2)*EPS +(RHO/2)*L**2*U**2*XPROP+RHO/2*L**3*U*Q* ... 
XQDB/2*DBS +RHO/2*L**2*U**2*XDBDB/2*DBS**2+ . . . 
RHO/2*L**2*XWDB/2*DBS*U*W 

sway FORCE 

FP(2) = -MASS*U*R + MASS*XG*P*Q + MASS*YG*R**2 - MASS*ZG*Q*R +... 

(RHO/2)*L**4*(YPQ*P*Q + YQR*Q*R)+(RHO/2)*L**3*(YP*U*P +... 
YR*U*R + YVQ*V*Q + YWP*W*P + YWR*W*R) + (RHO/2)*L**2* ... 

(YV*U*V + YVW*V*W +YDR*U**2*DR) -LATYAW + (WEIGHT-BOY) *.. . 
COS(THETA)*SIN(PHI) 

heave FORCE 

FP(3) = MASS*U*Q - MASS*V*P - MASS*XG*P*R - MASS*YG*Q*R + 

MASS*ZG*P**2 + MASS*ZG*Q**2 + (RHO/2) *L**4*(ZPP*P**2 +... 
ZPR*P*R + ZRR*R**2) + (RHO/2)*L**3*(ZQ*U*Q + ZVP*V*P +... 
ZVR*V*R) +(RHO/2)*L**2’^(ZW*U% + ZW*V**2 + U**2*(ZDS*...- 
DS+ZDB/2*DBP))-NORPIT+(WEIGHT-BOY)*COS(THETA)*COS(PHI)+. . . 
(RHO/2)*L**3^ZQN*U*Q*EPS +(RHO/2)*L**2*(ZWN*U*W +ZDSN*... 
U**2*DS)*EPS+ RHO/2^L**2*U**2*ZDB/2*DBS 

ROLL FORCE 

FP(4) = -IZ*Q*R +IY*Q*R -IXY*P*R +IYZ*0**2 -IYZ*R**2 +IXZ*P*Q +.. 
MASS*YG*U*Q -MASS*YG*V*P -MASS*2G*W*P+(RHO/2)*L**5*(KPQ*. . 
P*Q + KQR*Q*R) +(RHO/2)*L**4*(KP*U*P +KR*U*R + KVQ*V*Q +.. 
KWP*W*P + KWR*W*R) +(RHO/2)*L**3*(KV*U*V + KVW*V*W) + ... 

(YG*WEIGHT - YB*BOY)’^COS (THETA) *COS (PHI) - (ZG*WEIGHT - .. 
ZB*BOY ) *COS ( THETA )*S IN (PHI) + (RHO/2)*L**4*KPN*U*P*EPS +. . 
(RHO/2)*L**3*U**2%PROP +HASS*ZG*U*R+ ... 

■ RHO/2*L**3*U**2*(KDB/2*DBP-KDB/2*DBS) 

PITCH FORCE 

FP(5) = -IX*P*R +IZ*P*R +IXY*Q*R -IYZ*P*Q -IXZ*P**2 +IXZ*R**2 -.. 
MASS*XG*U*Q + MASS*XG*V*P + MASS*ZG*V*R - MASS*ZG*W*Q +... 
(RHO/2)*L**5*(HPP*P**2 +MPR*P*R +HRR*R**2)+(RHO/2)*L^*4*. . 
(MQ*U*6 + MVP’^V*P + MVR*V*R) + (RHO/2)*L**3* (MW*U’'W + ... 
MVV*V*^2+U**2*(MDS*DS+MDB/2*DBP))+ NORPIT -(XG*WEIGHT- ... 
XB*BOY ) *COS( THETA )*COS( PHI) +... 

(RHO/2)*L**3*(MWN*U*W+MDSN*U**2*DS)*EPS+ RHO/2*L**3*. . . 

U** 2*MDB / 2*DBS - ( ZG*WE I GHT - ZB *BOY ) *S IN ( THETA ) 

YAW FORCE 

FP(6) = -IY*P*Q +IX*P*Q +IXY*P**2 -IXY*Q**2 +IYZ*P*R -IXZ*Q*R -.. 
MASS*XG^U*R + MASS*XG*W*P - MASS*YG*V*R + MASS*YG*W^Q +... 
(RHO/2)*L**5*(NPQ*P*Q + NQR*Q*R) +(RHO/2)*L**4*(NP*U^P+. . . 
NR*U*R + NVQ*V*Q +NWP*W*P + NWR%*R) +(RHO/2)*L^*3*(NV*. . . 
U*V + NVW*V*W + NDR*U**2*DR) - LATYAW + (XG*WEIGHT - ... 

XB*BOY)*COS(THETA)*SIN(PHI)+(YG*WEIGHT)*SIN(THETA). . . 

+ (RHO/2)*L*'*:3*U**2*NPROP-YB*BOY*SIN(THETA) 

IF(Z.EQ.50.0)THEN 

WRITE (8,500)(FP(I), I = 1,6) 

Z = 0.0 
END IF 



NOW COMPUTE THE F(l-6) FUNCTIONS 



600 

* 

* 

* 

* 



DO 600 J = 1,6 

F(J) = 0.0 
DO 600 K = 1,6 

F(J) = MMINV(J,K)*FP(K) + F(J) 

CONTINUE 

THE LAST SIX EQUATIONS COME FROM THE KINEMATIC RELATIONS 
FIRST SET THE DRIFT CURRENT VALUES 



122 



UCO =0.0 
VCO =0.0 
WCO =0.0 

INERTIAL POSITION RATES F(7-9) 

F(7) = UCO + U*COS(PSl)*cds (THETA) + V*(COS(PSI)*SIN(THETA)*. . 

SIN(PHI) - SIN(PSI)*COS(PHI)) + W*(COS(PSI)*SIN(THETA)* 
COS(PHI) + SIN(PSI)*SIN(PHI)) 

F(8) = VCO + U*SIN(PSI)*COS (THETA) + V*(SIN(PSI)*SIN(THETA)*. . 

SIN(PHI) + COS(PSI)*COS(PHI)) + W*(SIN(PSI)*SIN(THETA)* 
COS(PHI) - COS(PSI)*SIN(PHI)) 

F(9) = WCO - U*SIN(THETA) +V*COS (THETA)*SIN(PHI) +W*COS (THETA) 
COS (PHI) 

EULER ANGLE RATES F(10-12) ■ 

F(10) = P + Q*SIN(PHI)*TAN(THETA) + R*COS (PHI )*TAN (THETA) 

F(ll) = Q*COS(PHI) - R*SIN(PHI) 

F(12) = Q*SIN(PHI)/COS (THETA) + R*COS (PHI) /COS (THETA) 



'k 

k 



^00 



k 

k 



k 






k 

k 

k 



IF (Z.EQ.l.O)WRITE (9 , 500) (F(I ) , I = 1,12) 
FORHAT(6E12.4) 

Z = Z + 1 



UDOT = F(l) 

VDOT = F(2) 

WDOT = F(3) 

PDOT = F(4) 

QDOT = F(5) 

ROOT = F(6) 

XDOTA= F(7) 

YDOT = F(8) 

ZDOT = F(9) 

PHIDOT = F(10) 

THETAD = F(ll) 

PSIDOT = F(12) 

U = INTGRL( 6.0, UDOT) 

V = INTGRL( 0.0, VDOT) 

W = INTGRL( 0.0, WDOT) 

P = INTGRL( 0.0, PDOT) 
d = INTGRL( 0.0, QDOT ) 

R = intgrl(o.o,rdot) 

XPOS = INTGRL(0.0,XDOTA) 
YPOS = INTGRL( 0.0, YDOT) 

ZPOS = INTGRL( 0.0, ZDOT) 

PHI = INTGRL( 0.0, PHIDOT) 
THETA = INTGRL( 0.0, THETAD) 
PSI = INTGRL( 0.0, PSIDOT) 

ZNEW = -ZPOS 

PHIANG = PHI/0.0174532925 
THEANG = THETA/0.0174532925 
PSIANG = PSI/0. 0174532925 



DYNAMIC 

* 

* 



IF (IFLAG.EQ.O.AND. JFLAG.EQ.O) THEN 
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UM0D(4)= 15.0*0.0174532925 
UMOD(3) = -15.0*0.0174532925 
UMOD(2) = -15.0*0.0174532925 
END IF 

IF(IFLAG.EQ.O.AND.ABS(THMANG) .GT.37.0) THEN 
ZCHG = X(9) - 5.0 

I FLAG = I FLAG + 1 
ENDIF 

IF (IFLAG.GT.O.O.AND.JFLAG.EQ.O) THEN 
UM0D(4)= 2.05*0.0174532925 
UM0D(2) = 0.0 
UM0D(3) = 0.0 
ENDIF 

IF (IFLAG.GT.O. AND. ZCHG. GT.ZDEPTH) THEN 
UM0D(4)= -11.0*0.0174532925 
UM0D(3) = 11.0*0.0174532925 
UM0D(2) = 11.0*0.0174532925 
ENDIF 

IF (ZDEPTH.LT.3.0.AND.ABS(THNANG) .LT.4.10) THEN 
UM0D(4)= 0.0 
UM0D(3) = 0.0 
UM0D(2) =0.0 
JFLAG = JFLAG + 1 
ENDIF 

IF (JFLAG. GT.O) THEN 
UM0D(4)= 0.0 
UM0D(3) = 0.0 
UM0D(2) = 0.0 
ENDIF 



i,QM 

JP0S(LI=1,UN=FT) . . . 



CONTROL FINTIM =200.00,DELT = .01 
SAVE .20,XPOS,XPOSM,U,UM,ZPOS,ZPOSM,W,WM,DBPM, . . . 

DBS , DBSM , DS , DSM , THEANG , THMANG , Q , QM 
PRINT 2.0,XPOS,XPOSM,U,UM,ZPOS,ZPOSM,W,WM,DBPM, . . . 

DBS , DBSM , DS , DSM , THEANG , THMANG , C ' ‘ ' 

GRAPH (G1,DE=TEK618) TIME(NI=10,UN=SEC) I 

ZP0SM(LI=2,UN=FT) 

TIME(NI=10,UN=SEC) W(LI=1 ,UN= ' FT/SEC ' ) . . . 
WM(LI=2',UN='FT/SEC ) 

TIME(NI=10,UN=SEC) Q(LI=1 ,UN= ' RAD/SEC ' ) . . . 
QM(LI=2, UN=' RAD/SEC ' ) 

TIME(NI=10,UN=SEC) THEANG(LI=1 ,UN=DEG) . . . 
THMANG (L 1=2, UN=DEG) 

XPOS(UN=FT) ZPOS(UN=FT) 

XPOSM(UN=FT) ZPOSM(UN=FT) 

TIME(NI=10,UN=SEC) DBS (LI=1 ,UN=RADIANS) . . . 
DBSM (LI=2 , UN=RADIANS ) 

GRAPH (G8,DE=TEK618) TIME (NI=10 ,UN=SEC) DS (LI=1 ,UN=RADIANS) . . . 

DSM (LI=2 , UN=RADIANS ) 

'DEPTH VS TIME) 

HEAVE VS TIME) 

PITCH RATE VS TIME) 

PITCH ANGLE VS TIME) 

ACTUAL DIVE PROFILE) 

MODEL DIVE PROFILE) 

BOW PLANE ANGLE) 

STERN PLANE ANGLE) 



GRAPH 

GRAPH 

GRAPH 

GRAPH 

GRAPH 

GRAPH 



(G2,DE=TEK618) 

(G3,DE=TEK618) 

(G4,DE=TEK618) 

G5,DE=TEK618) 

G6,DE=TEK618) 

G7,DE=TEK618) 



LABEL 

LABEL 

LABEL 

LABEL 

LABEL 

LABEL 

LABEL 

LABEL 

END 

STOP 



G1,DE=TEK618 

G2,DE=TEK618 

G3,DE=TEK618 

G4,DE=TEK618 

G5,DE=TEK618 

G6,DE=TEK618 

G7,DE=TEK618 

(G8,DE=TEK618) 
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******SUBROUTINE ETAT**************************************************** 

•k-k-k-k-k-k7K-k:k:k-k-^:k-k:k-k^:k-k-ki^-k-k-k-k-ki^-k-k-k-k-k-k-k'k^'k-k-k-k-k‘k'i^‘k‘k:ki^‘k'ki^‘M‘k'k‘k‘k:k‘k‘k-k-k-k-ki:-fck-ki<-k-k:k:fcki: 



C 

C 

c 

c 



10 



15 



16 

C 

C 

C 



PROGRAM ETAT 
IMPLICIT REAL*8(A-H,0-Z) 

COMMON/SYST/A(40 , 40 h B (40 , 40 ) , C (40 , 40 ) , D (40 , 40 ) 
COMMON/ STATES /X( 40) ,Y(40) , U(40) , W(40) 
COMMON/DIM/N , M , NR , NKS , EPS 
COMMON/ DIMl/DT 
COMMON/ FLAGS/ lOPT ( 5 ) 

COMMON/OPTIM/Q(24,24) ,R(24,24) 

DATA EPS/1. OE-7/ 

INITIALIZE ALL SYSTEM MATRICES 

EPS = l.OE-7 

DO 10 1=1,40 

X(I)=0.0 

Y(I)=0.0 

U(I)=0.0 

CONTINUE 

DO 15 1=1,40 

DO 15 J=l,40 

A(I,J)=0.0 

B(I,J)=0.0 

C(I,J)=0.0 

D(I,J)=0.0 

CONTINUE 

DO 16 1=1,24 

DO 16 J=l,24 

Q(I,J)=0.0 

R(,I,J)=0.0 

CONTINUE 



SET UP COEFICIENT MATRICES , INPUTS , INITIAL CONDITIONS 



CALL INPUT 
CALL MTXEXP 
CALL ROOTS 
DO 20 K=1,NKS 
CALL EXCIT(K) 
CALL UPDAT(K) 
CALL POUT(K) 
20 CONTINUE 





IF 


(IOPT(l 


■).EQ.l) 


CALL 


OPTIMA 


c 


IF 


(I0PT(1 


• Keq.i) 


CALL 


POUTOP 



END 

A :i*c A :^c :*r 



c 

c 



SUBROUTINE INPUT 
IMPLICIT REAL*8(A-H,0-Z) 



COMMON/SYST/A(40,40^ ,B(40,40) ,C(40,40) ,D(40,40) 
COMMON/STATES/X(40) ,Y(40) ,U(40) ,W(40) 



COMMON/DIM/N, M, NR, NKS, EPS 
COMMON/DIMl/DT 
COMMON/ FLAGS/ I OPT ( 5 ) 
COMMON/OPTIM/Q(24,24) ,R(24,24) 
OPEN(UNIT=5 , FILE= ' FILE ' , STATUS= ' OLD 
OPEN(UNIT=6 , FILE= ' FILE ' , STATUS= ' OLD 



READ (5,10 
WRITE (6, 10 
READ (5,9) 



N,L,M,K,NKS,IOPT(l 

N,L,M,K,NKS,IOPT(' 

NAS 



H: 



DT 

DT 






WRITE (6, 9) NAS 
DO 11 11=1, NAS 
READ (5,25) I,J,A(I,J) 
WRITE(6,25) I,J,A(I,J) 
11 CONTINUE 

READ (5,9) NBS 
WRITE (6, 9) NBS 
DO 12 11=1, NBS 
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READ (5,25 


) I,J,B(I,J) 




WRITE (6, 25 


) I,J,B(I,J) 


12 


■ CONTINUE 






READ (5,9) 


NCS 




WRITE (6, 9) 


NCS 




DO 13 11=1 


,NCS 




READ (5,25 


) I,J,C(I,J) 




WRITE (6, 25 


) I,J,ch,J) 


13 


CONTINUE 






READ (5,9) 


NDS 




WRITE(6,9) 


NDS 




DO 14 11=1 


,NDS 




READ (5,25 


) I,J,D(I,J) 




WRITE (6, 25 


) I,J,Da,J) 


14 


CONTINUE 






READ(5,9) : 


NXS 




WRITE (6, 9) 


NXS 




DO 35 11=1 


,NXS 




READ(5,25) 


I,J.X(I) 




WRITE(6,25) I,J,X(I) 


35 


CONTINUE • 






IF(IOPT(l) 


.NE.l) GO TO 




READ(5,9) 


NOS 




WRITE (6, 9) 


NOS 




DO 150 11= 


1,NQS 




READ (5, 25) 


I,J,Q(I,J)^ 




WRITE (6, 25 


) I,J,Q(I,J) 


150 


CONTINUE 






READ(5,9) 


NRS 




WRITE(6,9) 


NRS 




DO 180 11= 


1,NRS 




READ(5,25) 


I,J,R(I, J) 




WRITE(6,25) I,J,R(I,J) 


180 


CONTINUE 






NR=L 




9 


FORMAT (5X, 


15) 


10 


FORMAT (5X, 


6I5,E10.4) 


25 


FORMAT (15 


,I5,E10.4) 


190 


RETURN 






END 





SUBROUTINE MTXEXP 
IMPLICIT REAL*8(A-H,0-Z) 

COMMON/SYST/A(40,40) ,B(40,40) ,C(40,40) ,D(40,40) 
COMMON/DIM/N , M , NR , NKS , EPS 
COMMON/DIMl/DT 
C0MM0N/ETT/E(40,40) ,H(40,40) 

DIMENSION DD(40,40) ,L(50),RHO(50,2) ,W(50) 

MK=30 

DO 20 1=1, N 
L(I)=1 

RHO(I,1)=1.0 
DO 20 J=1,N 

IF(I.EQ.J) GO TO 10 
E(I,J)=0.0 
DD(I,J)=0.0 
H{I,J)=0.0 
GO TO 20 
10 CONTINUE 

E(I,J)=1.0 
DD(I,J)=1.0 
H(I,J)=DT 
20 CONTINUE 

MM=0 
K=1 
X=DT 

30 CONTINUE 

DO 80 1=1, N 

IF(L(I) .EQ.O) GO TO 80 
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DO 40 J=1,N 
W(J)=0.0 
40 CONTINUE 

DO 60 KK=1,N 
Y=DD(I,KK) 

IF(Y.EQ.O.O) GO TO 60 
DO 50 J=1,N 
W(J)=W(J)+Y*A(KK,J) 

50 CONTINUE 

60 CONTINUE 

DO 70 J=1,N 
DD(I,J)=W(J)*X 
70 CONTINUE 

80 CONTINUE 

K=K+1 
X=K 

X=DT/X 

DO 100 1=1, N 
IF(L(I) .EQ.O) GO TO 100 
Y1P=0.0 
DO 90 J=1,N 



Y1P=Y1P+ABS 
90 CONTINUE 

RH0(I,2)=Y1P 

IFUbS((RHO(I,2)-RHO(I,1))/RHO(I,2)) .GT.EPS) GOTO 100 

L(I)=0 

MM=MM+1 



J)+DD(I,J) 

j)+X*DDtl,J) 

(E(I,J)) 



100 



110 

120 



125 

135 

190 

195 

200 

130 

140 



CONTINUE 

IF(K.GT.MK) GO TO 130 
IF(MM.EQ.N) GO TO 120 
DO 110 1=1, N 
RH0(I,1)=RH0(I,2) 

CONTINUE 
GO TO 30 
CONTINUE 
DO 125 1=1, N 
DO 125 J=1,N 
DD(I,J)=0.0 
DO 125 K=1,N 

dd(i,j)=ddU , J)+h(i,k)*b(k, J) 

CONTINUE 
DO 135 1=1, N 
DO 135 J=1,N 
H(I,J)=DD(I,J) 

CONTINUE 
WRITE (6,190) 

FORMAT ( 5X, 'P -MATRIX' ,/) 

WRITE(6,200) ((E(I,J),J=1,N) ,I=1,N) 

WRITE (6,195) 

FORMAT ( 5X , ' QB-MATRIX ' , / ) 

WRITE(6,200J ((H(I,J),J=1,N),I=1,N) 

FORMAT (6E 12, 4) 

RETURN 

CONTINUE 

WRITE(6,140) MK ,EPS 
STOP 

FORMAT (IX, 'MATRIX EXPONENTIAL FAILED TO CONVERGE AFTER 
1' ITERATIONS' ,/, IX, 'CONVERGENCE FACTOR' ,E12 .4) 

END 



,14, 



ROOTS 



:A: :A: :*r :*r :*r :*r :*r :*r 



SUBROUTINE ROOTS 
IMPLICIT REAL*8(A-H,0-Z) 

COMPLEX*16 ZZ 

COMMON/SYST/A(40,40) ,B(40,40) ,C(40,40) ,D(40,40) 

COMMON/DIM/N , M , NR , NKS , EPS 

COMMON/DIMl/DT 

DIMENSION W(80) ,ZZ(40,40) ,WK(3200) 
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•DIMENSION XX(40,40) ,RZ(3200) 
EQUIVALENCE (ZZU,1), RZ(1)) 
IJOB=2 
IZ=40 



IA=40 



10 



15 



20 



25 

4 

3 



8 



DO 10 1=1,3200 

WK(I)=0.0 

CONTINUE 

DO 15 1=1,80 

W(I)=0.0 

CONTINUE 

DO 20 1=1,40 

DO 20 J=l,40 

ZZ(I, J)=0.0 

CONTINUE 

DO 25 1=1, N 

DO 25 J=1,N 

XX(I,J)=A(I,J) 

CONTINUE 



DO 4 I = 1,N 

WRITE(6,3)(XX(I,J), J=1,N) 

FORMAT (6E 12. 4) 

CALL EIGRF(XX,N,IA,IJOB,W,RZ,IZ,WK,IER) 
WRITE(6,8)(W(I), I = l,80j 
FORMAT (4E 12. 4) 

N2=N*2 



DO 30 1=1, N2, 2 
W1=W(I) 

11=1 + 1 
W2=W(Ii; 



30 



40 

50 

100 

120 

130 



I2=(l+l)/2 

WRITE(6,100) I2,W1,W2 
DO 50 1=1, N 
DO 40 J=1,N 
XX(1,J)=REAL(ZZ(J,I)) 

XX(2,J)=DIMAG(ZZ(J,I)) 

WRITE (6,120) I,XXU,J) ,XX(2,J) 

CONTINUE 

FORMAT(5X, 'EIGENVALUES' ,10X, 'REAL PART' , 

IlOX, 'IMAGINARY PART' , / , 5X, 13 , 12X,E12 .4 , lOX, E12 .4) 
FORMAT (5X, 'EIGENVECTORS' , 15 , 5X, 2E12 .4) 

FORMAT ( 5X, ' lER AND PERFORMANCE INDEX ' , 15 , lOX, E12 .4) 
;6,130) IER,WK(1) 



WRITE 
RETURN 
END 

******SUBROUTINE eXCIT(K )************************************************ 
************************************************************************* 



SUBROUTINE EXCIT(K) 
IMPLICIT REAL*8U-H,0-Z) 



COMMON/SYST/A(40,40j ,B(40,40) ,C(40,40) ,D(40,40) 
COMMON/STATES/X(40),Y{4“ 



,Y(40) ,U(40) ,W(40) 
COMMON/DIM/N , M , nr , NKS , EPS 
COMMON/DIMl/DT 
T=DT*FLOAT(K) 

U(1)=0.0 

RETURN 

END 

5*c 5 UB ROUTINE UPDAT 

SUBROUTINE UPDAT (K) 

IMPLICIT REAL*8U-H,0-Z) 

COMMON/SYST/A(40,40) ,B(40,40) ,C(40,40) ,D(40,40) 
COMMON/STATES/X^O) , Y(40) ,U(40) ,W(40) 

COMMON/ D IM/N , M , NR , NKS , EP S 
COMMON/DIMl/DT 

COMMON/ETT/E(40,40) ,H(40,40) 

DIMENSION XS(40) ,XNUo) ,YS(40) ,YN(40) 
T=DT*FLOAT(K) 

DO 10 1=1, N 
XS(I)=0.0 
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XN(I)=0.0 
YS(I)=0.0 
YN(I)=0.0 
DO 10 J=1,N 

XS(I)=XS(I)+E(I, J)*X(J) 
XN(I)=XN(I)+H(I, J)*U(J) 
YS(I)=YS(I)+C(I, J)*X(J) 
YN(I)=YN(I)+D(I, J)*U(J) 
10 CONTINUE 



DO 


20 1= 


1 


rN 


xn 


[)=XS( 


i; 


KXN(I) 


yO 


[)=YS( 


i: 


>+YN(l) 



20 CONTINUE 
RETURN 
END 

******subroutine pout (K) ************************************************* 



SUBROUTINE POUT(K) 
IMPLICIT REAL*8(A-H,0-Z) 



COMMON/ SYST/A (40, 40 ) ,B(40,40) ,C(40,40) ,D(40,40) 
COMMON/STATES/X(40) ,Y(40) ,r 



, ,U(40),W(40) 

COMMON/ D IM/ N , M , NR , NKS , EPS 
COMMON/DIMl/DT 
T=FLOAT(K)*DT 
IF(K.EQ.l) WRITE (6,110) 

WRITE (6,100) T,Y(1) ,Y(2) ,Y(3) ,Y(4) 

100 FORMAT(5X,F10.2,5X,4E12.4) 

110 FORMAT('TIME' ,5X, 'XI' ,5X, 'X2' ,5X, ' X3 ' ,5X, 'X4' ,5X, ' U1 ' 

1,10X, 'U2' ,/) 

RETURN 

END 

* THIS SUBROUTINE SETS UP THE SYSTEM AND ADJOINT EQUATIONS * 

* MATRICES AS * 

* A -(B(R-l)BT) * 

* SS = * 

* -Q -AT * 

* AND FINDS THE EIGENVALUES /EIGENVECTORS OF SS . * 

* COLLECTING THE STABLE VECTORS AS IN POTTERS METHOD * 

* AND PARTITIONING, RESULTS IN THE SOLUTION OF THE * 



RICCATI EQUATION FOR THE OPTIMUM STATE FEEDBACK 
GAINS. THIS ROUTINE LIMITS A(24,24). 






SUBROUTINE OPTIMA 
IMPLICIT REAL*8(A-H,0-Z) 

COMPLEX*! 6 ZZ,W12,W22 

COMMON/SYST/aU0,40),B(40,40),C(40,40) ,D(40,40) 
COMMON/OPTIM/Q ( 24,24),R(24,24) 

COMMON/ DIM/N,M, NR, NKS, EPS 
COMMON/DIMl/DT 

DIMENSION SS(48,48),TEMP(24,24) ,ZZ 
DIMENSION W12(24,24) ,W22(24,24) ,RZ 
EQUIVALENCE (ZZ(1 ,1) ,RZ(1) ) 

IA=48 
IZ=48 
IJOB=l 
N2=2*N 
N4=2*N2 
N21=N2+1 
DO 1 1=1, N2 
DO 1 J=1,N2 
1 SS(I,J)=0.0 

DO 5 1=1, N 
DO 5 J=1,N 
5 TEMP(I,J)=0.0 

C WRITE(6,140) ((R(I, J) ,J=1,NR) ,I=1,NR) 

NDIM1=24 
NDIM2=48 

CALL INVERT (R , DET ,NR , NDIMl , NDIM2 ) 

C WRITE(6,150) ((R(I,J),J=1,NR),I=1,NR) 



48,48) ,W(96) ,WK(2400) 
4608) 
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10 



20 



30 



40 



50 

C 



CO 

60 

70 

C 

C 

C 



200 

210 

c 



220 



C 



230 



DO 10 1=1, NR 
DO 10 J=1,N 
DO 10 K=1,NR 

TEMP(I,J)=TEMP(I,J)+R(I,K)*B(J,K) 

DO 20 1=1, N 
DO 20 J=1,N 
JJ=J+N 

DO 20 K=1,NR 

SS(I,JJ)=SS(I,JJ)-B(I,K)*TEMP(K,J) 

DO 30 1=1, N 
DO 30 J=1,N 
SS(I,J)=A(I,J) 

DO 40 1=1, N 
DO 40 J=1,N 
II=I+N 
JJ=J+N 

SS(II,JJ)=-A(J,p 

SS(II,J)=-Q(I,J) 

CALL EIGRF(SS,N2,IA,IJOB,W,RZ,IZ,WK,IER) 
WRITE(6,90) 

DO 50 1=1, N4, 2 

I2=(I+p/2 

W1=W(I) 

11 = 1+1 

W2=W(I1) 

WRITE(6,120) I2,W1,W2 

WRITE(6,100) 

DO 70 J=1,N2 
DO 60 1=1, N2 
SS(I,i5=REAL(ZZ(I,J)) 

SS(I,2)=DIMAG(ZZ(I,J)) 

WRITE (6,120) J,SSU,1) ,SS(I,2) 

CONTINUE 

CONTINUE 

COLLECT ALL STABLE EIGENVECTORS INTO A V-MATRIX 
(USING SS(48,48)) , PARTITION, AND SOLVE FOR THE 
SOLUTION OF THE RICCATTI EQUATION . 

J=0 

DO 210 IC=1,N4,2 
JC=(IC+l)/2 
IF(W(IC) .GE.0.0) 

J=J+1 



GO TO 210 



DO 200 1=1, N 
IPN=I+N 

W12(I,J)=ZZ(I,JC) 

W22(I,J)=ZZ(IPN,JC) 

CONTINUE 

CONTINUE 

INVERT COMPLEX W12(N,N) 

DO 220 1=1, N 
DO 220 J=1,N 
IPN=I+N 



JPN=J+N 

SS(I,J)=REAL(W12(I,J)) 
SS(IPN,J)=DIMAG(W12(I, J)) 
SS(I,JPN)=-SS(IPN,J) 
SS(IPN,JPN)=SS(I,J) 

CONTINUE 

NDIM1=48 

NDIM2=96 

CALL INVERT ( SS , DET , N2 , NDIMl , NDIM2 ) 
FORM W22*(W12)-1=P 

DO 230 1=1, N 
DO 230 J=1,N 
IPN=I+N 



Q I,J =SS I,J) 
R(I,J)=SS(IPN,J) 
DO 240 1=1, N 
DO 240 J=1,N 
SS(I, J)=0.0 
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240 

C 



250 

C 



DO 240 K=1,N 

SS(I,J)=SS(I,J)+REAL(W22(I,K))*Q(K,J)-DIMAG(W22(I,K))*R(K, J) 
CONTINUE 



FORM GAIN MATRIX INTO THE Q ARRAY 

DO 250 1=1, NR 
DO 250 J=1,N 
Q(I,J)=0.0 
DO 250 K=1,N 

Q(I, J)=Q(I,J)+TEMP(I,K)*SS(K,J) 

CONTINUE 



COMPUTE THE CLOSED LOOP A-MATRIX 



DO 260 1=1, N 
DO 260 J=1,N 
DO 260 K=1,NR 

260 A(I,J)=A(I,J)-B(I,K)*Q(K,J) 

WRITE(6,270) 

DO 265 K=1,NR 

265 WRITE(2,275) (Q(K, J) , J=1 ,N) 

C WRITE(6,280) 

C DO 285 1=1, N 

C85 WRITE(6,275) (A(I , J) , J=1 ,N) 

C CALL ROOTS 

90 FORMAT(5X, ' EIGENVALUES-SYSTEM+ADJOINT- ' ) 

100 FORMAT(5X, 'EIGENVECTORS RE/IMAG') 

120 FORMAT(5X,I5,10X,E12.4,10X,E12.4) 

150 FORMAT(5X, 'R-INVERSE' ,/,4E12.4) 

140 FORMAT(5X, 'R-MATRIX' ,/,4E12.4) 

270 FORMAT (5X, 'TOTAL STATE FEEDBACK GAIN MATRIX',/) 

275 FORMAT (3E20. 10) 

280 FORMAT (5X, 'CLOSED LOOP A-MATRIX',/) 

RETURN 

END 



SUBROUTINE INVERT ( A, DET,N,NDIM1 ,NDIM2) 

IMPLICIT REAL*8(A-H,0-Z) 

C THIS ROUTINE INVERTS A SQUARE MATRIX USING 

C GAUSS ELIMINATION. THE ORIGINAL MATRIX IS DESTROYED 

C AND ITS INVERSE IS RETURNED AS 'A'. 

DIMENSION A(NDIM1,NDIM2) 

NDIGIT=30 

SUM=0.0 

DO 10 1=1, N 

DO 10 J=1,N 

SUM=SUM+ABS(A(I,J)) 

10 CONTINUE 

SUM=10.0**(-NDIGIT/2. )*SUM/N**2 

NP1=N+1 

NPN=N+N 

DO 20 1=1, N 

IPN=I+N 

DO 20 J=NP1,NPN 

au,J)=o.o 

IF (IPN.EQ.J) A(I,J)=1.0 
20 CONTINUE 

C DO 25 1=1, N 

C WRITE(6,900) (A(l , J) , J=1 ,NPN) 

25 CONTINUE 

DET=1. 

INTCH=0 
DO 90 1=1, N 
IP1=I+1 

IF (I.EQ.N) GO TO 50 
M=I 

DO 30 J=IP1,N 

IF (ABS(A(M,I)) .LT.ABS(A(J,I))) M=J 
30 CONTINUE 

IF (M.EQ.I) GO TO 50 
INTCH=INTCH+1 
DO 40 J=1,NPN 
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TEMP=A(M,J) 

a7m,j)=a(i,j) 

A(I,J)=TEMP 
40 CONTINUE 

50 CONTINUE 

IF(A(I,I) .EQ.0.0) GO TO 110 - 

IF (ABS(A(I,I)) .LT.SUM) WRITE(6,140) 

DO 60 J=IP1,NPN 
A(I,J)=A(I,J)/A(I,I) 

60 CONTINUE 

DO 80 J=1,N 
IF (J.EQ.I) GO TO 80 
DO 70 K=IP1,NPN 
A(J,K)=A(J,K)-A(J,I)*A(I,K) 

70 CONTINUE 

A(J,I)=0.0 
80 CONTINUE 

DET=DET*A(I,I) 

90 CONTINUE 

DET= ( - 1 ) **INTCH*DET 
DO 100 1=1, N 
DO 100 J=1,N 
A(I,J)=A(I,J+N) 

100 CONTINUE 

C DO 26 1=1, N 

C WRITE(6,910) (A(I,J) ,J=1,NPN) 

26 CONTINUE 

RETURN 

110 CONTINUE 

WRITE (6, 130) 

DO 120 1=1, N 
DO 120 J=1,N 

aU,J)=i*o 

120 CONTINUE 

RETURN 

130 FORMAT (5X, 'THE MATRIX IS SINGULAR, NO SOLUTION HAS BEEN FOUND') 

140 FORMAT (5X, 'THE MATRIX IS ILLCONDITIONED ' ) 

900 FORMAT(2X, 'ABEF. ' ,6E12.3) 

910 FORMAT (2X, 'AAFT. ' ,6E12. 4) 

END 

:ki::k-k:k:k:k'k:k'ki::k'ki^:k:k:k'k:k:k:k:k:k'k:k:k-k:k9::k'k:k:k'k:k:k-k:k:ki^i^i^:ki^ 

SUBROUTINE POUTOP 
IMPLICIT REAL*8U-H,0-Z) 

RETURN 

END 
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